% function [HeadposeWithIDs, newIDstatus] = assignIDs(currentHeadpose,previouseHeadpose,IDstatus,NewHeadPoses)
% L = length(NewHeadPoses);
relax = 3;

L = frameIndex;
HeadposeWithIDs = [];
if ~isempty(currentHeadpose)
    Nc = size(currentHeadpose,2);
    HeadposeWithIDs = [currentHeadpose;zeros(1,Nc)];
    
    % copy the IDs from the previouse frames (-1) if the distance is very close
    if ~isempty(previouseHeadpose)
        Np = size(previouseHeadpose,2);
        Dis = zeros(Np,Nc);
        
        for i = 1:Nc,
            for j = 1:Np,
                Dis(j,i) = sum(abs(currentHeadpose(:,i)-previouseHeadpose(1:3,j)));
            end
        end
        
        for j = 1:Np,
            [minV,minInd] = min(Dis(j,:));
            if minV<0.05*relax,
                % assign the ID
                HeadposeWithIDs(4,minInd) = previouseHeadpose(4,j);
            end
        end
    end
    
    AssignedIndex = HeadposeWithIDs(4,:)~=0;
    AssignedIDs = HeadposeWithIDs(4,AssignedIndex);
    
    % there are still IDs not assigned after compare to the previous frame
    notAssignedIndex = find(HeadposeWithIDs(4,:)==0);
    if length(notAssignedIndex)>0,
        for i = 1:length(notAssignedIndex)
            currNotAssIndex = notAssignedIndex(i);
            %currentHeadpose(:,currNotAssIndex)
            % try to find the near points from IDstatus
            if ~isempty(IDstatus),
                IDconsider = [];
                for j = 1:length(IDstatus)
                    idstat = IDstatus{j};
                    currentID = idstat.id;
                    use = 1;
                    if ~isempty(AssignedIDs)
                        if any(currentID == AssignedIDs),
                            use = 0;
                        end
                    end
                    if use
                        Timediff = idstat.Timediff;
                        if Timediff<=5,
                            dis = norm(idstat.pos-currentHeadpose(:,currNotAssIndex),'fro');
                            
                            if dis<0.1*relax,
                                temp = [dis,Timediff,currentID];
                                IDconsider = [IDconsider;temp];
                            end
                        end
                    end
                end
                
                if ~isempty(IDconsider),
                    [~,useIndex] = min(IDconsider(:,1));
                    % assign the ID
                    assinedID = IDconsider(useIndex,3);
                    HeadposeWithIDs(4,currNotAssIndex) = assinedID;
                    AssignedIDs = [AssignedIDs,assinedID];
                end
            end
            
        end
    end
    
     
    
    
    % there are still IDs not assigned after comparison to the recent 4 frames
    notAssignedIndex = find(HeadposeWithIDs(4,:)==0);
    if length(notAssignedIndex)>0,
        for i = 1:length(notAssignedIndex)
            currNotAssIndex = notAssignedIndex(i);
            %currentHeadpose(:,currNotAssIndex)
            % try to find the near points from IDstatus
            if ~isempty(IDstatus),
                IDconsider = [];
                Dis = [];
                for j = 1:length(IDstatus)
                    idstat = IDstatus{j};
                    currentID = idstat.id;
                    use = 1;
                    if ~isempty(AssignedIDs)
                        if any(currentID == AssignedIDs),
                            use = 0;
                        end
                    end
                    if use
                        Timediff = idstat.Timediff;
                        if Timediff<=30, % 1s
                            dis = norm(idstat.pos-currentHeadpose(:,currNotAssIndex),'fro');
                                temp = [dis,Timediff,currentID];
                                IDconsider = [IDconsider;temp];
                        end
                    end
                end
                
                if ~isempty(IDconsider),
                    [~,useIndex] = min(IDconsider(:,1));
                    % assign the ID
                    if IDconsider(useIndex,1)<0.5 && sum(IDconsider(:,1)>IDconsider(useIndex,1)+0.3)==size(IDconsider,1)-1,            
                        assinedID = IDconsider(useIndex,3);
                        HeadposeWithIDs(4,currNotAssIndex) = assinedID;
                        AssignedIDs = [AssignedIDs,assinedID];
                    end
                end
            end
            
        end
    end
        
        
    % there are still IDs not assigned after comparison to the recent 4 frames
    notAssignedIndex = find(HeadposeWithIDs(4,:)==0);
    if length(notAssignedIndex)>0,
        
        if ~isempty(IDstatus),

            %% occlusion detection
            % If these unsigned targets are occluded or not in the x-z plane 
            
            % first get the IDs that might be occluded
            savedIDs = [];
            for j = 1:length(IDstatus)
                idstat = IDstatus{j};
                savedIDs = [savedIDs, idstat.id];
            end
            MightOccludedIDs = setdiff(savedIDs,AssignedIDs);
            
            
            [~,solveOrder] = sort(HeadposeWithIDs(3,notAssignedIndex),'ascend');
            % first find the unsigned target that is in front
            for i=1:length(notAssignedIndex),
                currnotAssignedIndex = notAssignedIndex(solveOrder(i));
                
                % find the distance of the current un assigned target to
                % these might occluded targets
                if ~isempty(MightOccludedIDs)
                    Dis = [];
                    Timediff_array = [];
                    Pos_array = [];
                    %id_array = [];
                    for j = 1:length(IDstatus)
                        idstat = IDstatus{j};
                        if ismember(idstat.id,MightOccludedIDs);
                            % distance in the x-z plane
                            dis = norm(HeadposeWithIDs([1,3],currnotAssignedIndex)-idstat.pos([1,3]),'fro');
                            Dis = [Dis,dis];
                            Timediff_array = [Timediff_array,idstat.Timediff];
                            Pos_array = [Pos_array,idstat.pos(:)];
                            %id_array = [id_array,idstat.id];
                        end
                    end
                    
                    
                    [mindis,minindex] = min(Dis);
                    id = MightOccludedIDs(minindex);
                    Timediff = Timediff_array(minindex);
                    origFlag = 0;
                    %if mindis<min(0.3,0.03*max(Timediff,10)),
                    if (mindis<0.25 && sum(Dis>0.4)==length(Dis)-1) || (mindis<0.3 && sum(Dis>0.5)==length(Dis)-1)  || (mindis<0.4 && sum(Dis>0.6)==length(Dis)-1) || mindis<min(0.2,0.03*max(Timediff,5)),
                        %Originate from the same point
                        HeadposeWithIDs(4,currnotAssignedIndex) = id;
                        MightOccludedIDs = setdiff(MightOccludedIDs,id);
                        origFlag = 1;
%                     else % now we detect if occlusion happens or not
%                         if mindis<min(1,0.03*max(Timediff,10)),
%                             segnum = ceil()
%                         end
                    end
                    
                    if origFlag==0,% now we detect if occlusion happens or not
                        occFlag = 0;
                        [~,occOrder] = sort(Dis,'ascend');
                        for d = 1:length(MightOccludedIDs),
                            id = MightOccludedIDs(occOrder(d));
                            Timediff = Timediff_array(occOrder(d));
                            
                                
                            if Dis(occOrder(d))<2,%min(1,0.03*max(Timediff,30)),
                                segNum = ceil(Timediff/50);
                                TimeCentres = round(Timediff/(2*segNum)*(1:2:2*segNum-1));
                                occFlag_array = false(1,segNum);
                                
                                pos1 = Pos_array([1,3],occOrder(d));
                                pos2 = HeadposeWithIDs([1,3],currnotAssignedIndex);
                            
                                posCentres = repmat(pos1,[1 segNum])+(pos2-pos1)/(2*segNum)*(1:2:2*segNum-1);
                                for tIndex = 1:segNum,
                                    timeCen = TimeCentres(tIndex);
                                    posCen = posCentres(:,tIndex); % this point with origin [0 0] is the occluding line
                                    
                                    % equation 9 http://mathworld.wolfram.com/Point-PlaneDistance.html
                                    % the line is XCoeff*x + ZCoeff*z = 0
                                    XCoeff=posCen(2);ZCoeff=-posCen(1);
                                    denom = sqrt(XCoeff^2 + ZCoeff^2);
                                    
                                    TT = 10;
                                    startT = max(1,L-timeCen-TT);
                                    endT = min(L-timeCen+TT,L);
                                    detecSeg = NewHeadPoses([startT:endT]);
                                    detecSeg = cell2mat(detecSeg);
                                    detecSeg = detecSeg([1 3],:);
                                    del = detecSeg(2,:)>pos2(2)+0.5; % at lest one centre meter AFTER (should be before, but considering the delay...) the un assigned target
                                    detecSeg(:,del) = [];
                                    
                                    tempdis = abs(detecSeg'*[XCoeff;ZCoeff])/denom;
                                    
                                    % now calcu late the distance between
                                    % these lines to the occluding line
                                    if any(tempdis<0.3),
                                        occFlag_array(tIndex) = true;
                                    end
                                    
                                end
                                
                                if sum(occFlag_array)>=ceil(length(occFlag_array)*0.5), % half detected as occlusions
                                    HeadposeWithIDs(4,currnotAssignedIndex) = id;
                                    MightOccludedIDs = setdiff(MightOccludedIDs,id);
                                    occFlag = 1;
                                    break % break the for loop for d=...
                                end
                            end
                        end
                    end
 
                end
            end
            
            % if there are still unassigned target after occlusion
            % detection, we then assian new IDs to these targets
            notAssignedIndex = find(HeadposeWithIDs(4,:)==0);
            
            
            % assign new ID number here :) Finally!
            if length(notAssignedIndex)>0
                maxID = max(savedIDs); % max value in the existing IDs
                BetterUse = 1:maxID;
                ConsiderAssignedIDs = setdiff(BetterUse,savedIDs);
                
                if ~isempty(ConsiderAssignedIDs),
                    ConsiderAssignedIDs = ConsiderAssignedIDs(randperm(length(ConsiderAssignedIDs)));
                end
                ConsiderAssignedIDs = [ConsiderAssignedIDs,maxID+[1:5]]; % at most 5 emerging targets
                HeadposeWithIDs(4,notAssignedIndex) = ConsiderAssignedIDs(1:length(notAssignedIndex));
                %                 ConsiderAssignedIDs = maxID+[1:5];
                %                 HeadposeWithIDs(4,notAssignedIndex) = ConsiderAssignedIDs(1:length(notAssignedIndex));
            end
            
        else
            ConsiderAssignedIDs = [1:5]; % at most 5 emerging targets
            HeadposeWithIDs(4,notAssignedIndex) = ConsiderAssignedIDs(1:length(notAssignedIndex));
        end
          
    end
    
end


IDstatus = updateIDstatus(IDstatus,HeadposeWithIDs);
currentHeadpose=HeadposeWithIDs;

