
%% Same as in preprocessing individual:
[b,a]=butter(5,12/500);                                                    % filter parameters                                                                                                 % amount of trials
[targets,i] = unique(subject.data.TargetPosition(:,1));
targets(:,2) = subject.data.TargetPosition(i,2);
%% in case there is a missing value
if sum(subject.misses == 1)
    if  p
        figure('units','normalized','outerposition',[0 0 1 1])
    end
    disp('Check misses in data')
    data = subject.data;    
    nTrials = data.Trials;                                             
    [targets,i] = unique(data.TargetPosition(:,1));
    targets(:,2) = data.TargetPosition(i,2);
    
    
    disp(ns)
    ind = find(subject.misses == 1);                                        % trials where no samples could be selected
    for t = 1:length(ind)
        trial = ind(t);
        disp(trial)
        frames = 1:data.FrameData.Frames(trial);                            % index of frames of the current trial
        px = data.FrameData.RobotPosition(trial,frames,1);                  % x position data of current trial
        py = data.FrameData.RobotPosition(trial,frames,2);                  % y position data of current trial
        pt = data.TargetPosition(trial,1:2);                                % target location
        time = data.FrameData.TrialTime(trial,frames);                      % time vector
        State = data.FrameData.State(trial,frames);                         % States of this trial and frames
        %%
        if strcmp(forcetype,'Handle')                                                                                                               
            ChannelForces = data.FrameData.ChannelHandleForce(trial,frames);% Forces measured at the handle
        elseif strcmp(forcetype,'Robot')                                                                                                            
            ChannelForces = data.FrameData.ChannelForces(trial,frames);     % Forces measured at the robot
        end
        %%  we first interpolate to true 1000 Hz (i..)
        itime = 0:0.001:max(time);                                         % time to which we interpolate
        ipx = interp1(time,px,itime,'spline','extrap');                    % interpolated x-position
        ipy = interp1(time,py,itime,'spline','extrap');                    % interpolated y-position
        iF = interp1(time,ChannelForces,itime,'linear','extrap');          % interpolated Forces
        iState = interp1(time,State,itime,'nearest','extrap');             % interpolated States
        %% and filtered the interpolated position data (if...)
        ifpx = filtfilt(b,a,ipx);
        ifpy = filtfilt(b,a,ipy);
        %% velocity and speed
        vx = gradient(ifpx,1e-3);                                               % calculate x velocity from filtered and interpolated position data
        vy = gradient(ifpy,1e-3);                                               % calculate y velocity from filtered and interpolated position data
        speed= sqrt(vx.^2+vy.^2);                                               % calculate speed
        distance_to_target = sqrt((ifpx-pt(1)).^2+(ifpy-pt(2)).^2);             % distance to the target location
        ds_t = gradient(distance_to_target,1e-3);                               % ensuring the participant is moving towards the target location
        distance_to_start = sqrt((ifpx).^2+(ifpy).^2);                          % distance to the target location
        ds_s = gradient(distance_to_start,1e-3);                                % ensuring the participant is moving away from the start location
        speed_for_sample_selection = speed;
        speed_for_sample_selection(sign(-ds_t)>0|sign(ds_s>0)) = speed(sign(-ds_t)>0|sign(ds_s>0));
        speed_for_sample_selection(sign(-ds_t)<=0&sign(ds_s<=0)) = -speed(sign(-ds_t)<=0&sign(ds_s<=0));
        
        %% relevant samples
        
        raw_samples = 1:length(speed_for_sample_selection);
        % If there are blips before the start of the movement, take the next frame
        % after the last blip as the start of index of movement
        if ns ~= 5
            % If there are blips before the start of the movement, take the next frame
            % after the last blip as the start of index of movement
            if sum(diff(find(iState == 8))>1)>0
                sample_starts = find(iState == 8);
                raw_samples = sample_starts(find(diff(sample_starts)>1,1,'last')+1):raw_samples(end);                                                                       % if so take the next frame after the last blip as the start of index of movement
            end
        end
        
        
        % make sure we take the bellshape around peak velocity
        [v,i] = max(speed_for_sample_selection(raw_samples));
        [V,I] = findpeaks(speed_for_sample_selection(raw_samples),'MinPeakHeight',0.6*v); % find peaks of at least 60% of the maximum peak
        % check whether the movement was performed after stopmoving (pp 3);
        % val is the max peak of the full trial of all trials
        if i~=I(1) && ns ~= 12&& (ns ~= 21 && t == 2)
            disp('check this trial')
            i = I(1);
            v =V(1);
        end
        if ns == 4 && ind(t) == 11
            %remove backwards movement to find peak:
            
            [v,i] = max(speed(raw_samples(1:1037)));
            [V,I] = findpeaks(speed(raw_samples(1:1037)),'MinPeakHeight',0.6*v); % find peaks of at least 60% of the maximum peak
            % check whether the movement was performed after stopmoving (pp 3);
            % val is the max peak of the full trial of all trials
        end
        if i~=I(1) && ns ~= 12&& (ns ~= 21 && t == 2)
            disp('check this trial')
            i = I(1);
            v =V(1);
        end
        start = find(speed(raw_samples(1:i))<0.15*v,1,'last');
        finish_speed = find(speed(raw_samples(i:end))<0.15*v,1,'first');
        
        finish_sign = find(gradient(speed(raw_samples(i:end)))>0 & speed(raw_samples(i:end))<(0.5*v),1,'first');
        if (ns == 17 && t==1)
            finish = finish_speed;
        else
            finish = min([finish_speed,finish_sign]);
        end
        samples = raw_samples(start:(i+finish-1));
        
        %% saving the missing trials sample data:
        AI = mean(iF(samples)./(f*speed(samples)));
        subject.r(trial) =AI;
        subject.fpredicted{trial} =f*speed(samples);
        subject.f{trial} = iF(samples);
        subject.speed{trial} = speed(samples);
        n = length(samples);
        subject.posx(trial,1:n) = ipx(samples);
        subject.posy(trial,1:n) = ipy(samples);
        %% plotting
        if p == 1
            subplot(length(ind),2,(t-1)*2+1)
            plot(targets(:,1),targets(:,2),'o','color',[0.9290 0.6940 0.1250],'Markerfacecolor',[0.9290 0.6940 0.1250])
            hold on
            plot(pt(1),pt(2),'o','color',[0.8500 0.3250 0.0980],'Markerfacecolor',[0.8500 0.3250 0.0980])
            plot(px,py,'b-')
            hold on
            plot(ipx(samples),ipy(samples),'r--','linewidth',2)
            xlim([-12,12])
            ylim([-5,17])
            text(-10,-2,sprintf('participant: %d \n trial: %d/%d,\n trialtype %d',ns, trial,nTrials,data.TrialType(trial)))
            title('position')
            %legend('all possible targets','selected target','all position data','selected position data','Location','SouthEast')
            hold off
            
            subplot(length(ind),2,(t-1)*2+2)
            plot(speed,'b-')
            hold on
            plot(speed_for_sample_selection,'r-')
            plot(raw_samples(1)*[1,1],[-75,75],'k-')
            plot(raw_samples(end)*[1,1],[-75,75],'k-')
            plot(samples,speed(samples),'r--','linewidth',2)
            %plot(distance,'g-')
            title('speed')
            %legend('all speed data','speed including sign of direction','selected speed data','distance to target')
            hold off
            pause
        end
        
    end
end
