%Teerawat Kamnardsiri
%Colledge of Arts, Media and Technology, Chiang Mai University
%Running Approach of Long Jump
%01 Jan, 2019.

clc;
close all;
clear;

%Selecting a background video (.MOV)
BG_File='BG.MOV';

%Selecting a name of foreground video
FileName='T01_Run10';

FG_File=fullfile([FileName,'.MOV']);

%Distance of the camera around 18-20 m., Nikon 1 J1 with Lens 10-30 mm.
PIXAREA=80;

%*****************************************************************************************************************
%Read Background Image
BG= VideoReader(sprintf('%s%s',BG_File));
%Read Background Image
Background = read(BG,2);
Background2 = imresize(Background, 0.5);
Background_BW = rgb2gray(Background2);

%Calibration the system
%color = {'green','green','green','green'};
Background2 = insertText(Background2, [1 1], sprintf(' Knowledge-Based Smart Trainer for Long Jump Athletes Using Computer Vision Techniques\n Calibration the system, please perform 2 steps:\n 1: click mouse at the Marker M2 (Take-off board) and then\n 2: click mouse at the Marker M3 (5 maters from take-off board)for calibration the system.'));

imshow(Background2);


%Loading video
LongJump= VideoReader(sprintf('%s%s',FG_File));
LongJumpW= LongJump.Width*0.5;
LongJumpH= LongJump.Height*0.5;

Foreground=read(LongJump,2);
Foreground = imresize(Foreground, 0.5);
Foreground_BW = rgb2gray(Foreground);

%imshow(Foreground_BW);

Foreground_match = imhistmatch( Background_BW,Foreground_BW);
Mean_BG=mean2(Background_BW);
Mean_FG=mean2(Foreground_BW);
Mean_Match=mean2(Foreground_match);

Meandiffence1=(Mean_BG-Mean_FG);
Meandiffence2=(Mean_BG-Mean_Match);

%PANR between Background and Foreground images
[peaksnr, snr] = psnr(Background_BW, Foreground_match);

%Threshold setting at 60 (high light: mean= )and at 120(low light: mean= )
if (Mean_BG < 145)%Teerawat01
    %Background_BW=imgaussfilt(Background_BW,4);
    Background_BW = imadjust(Background_BW,[0.00; 0.40],[0.30; 1.00], 0.3);
    Threshold=35+Meandiffence2;

elseif (Mean_BG < 150)
    Background_BW=imgaussfilt(Background_BW,5);
    Background_BW = imadjust(Background_BW,[0.00; 0.4],[0.5; 1.0], 0.3); 
    Threshold=55+Meandiffence2;    
    
elseif (Mean_BG < 155)%eye01
    %Background_BW=imgaussfilt(Background_BW,1);
    Background_BW = imadjust(Background_BW,[0.00; 0.4],[0.1; 0.7], 0.3);  
    Threshold=60+Meandiffence2;
    
elseif (Mean_BG < 160)%Mod speed test & eye 14
    %Background_BW=imgaussfilt(Background_BW,5);
    Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.0; 0.7], 0.6);
    Threshold=75+Meandiffence2;
    
elseif (Mean_BG < 170)
    Background_BW=imgaussfilt(Background_BW,5);
    Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.0; 0.7], 0.6);
    Threshold=80+Meandiffence2;
    
elseif (Mean_BG < 180)
    Background_BW=imgaussfilt(Background_BW,5);
    Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.1; 0.8], 0.7);
    Threshold=90+Meandiffence2;

elseif (Mean_BG < 190)%Sirinun
    Background_BW=imgaussfilt(Background_BW,5);
    Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.1; 0.9], 1.0);
    Threshold=90+Meandiffence2;
    
elseif (Mean_BG < 200)%
    Background_BW=imgaussfilt(Background_BW,5);
    Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.1; 1.0], 1.5);
    Threshold=90+Meandiffence2;
    
elseif (Mean_BG < 210)%
    %Background_BW = imgaussfilt(Background_BW,5);
    Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.1; 0.8], 2.0);
    Threshold=95+Meandiffence2;
    
elseif (Mean_BG < 215)%Sirinun
    %Background_BW=imgaussfilt(Background_BW,5); 
    Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.1; 1.0], 2.0);
    Threshold=100+Meandiffence2;
    
elseif (Mean_BG < 220)%Teerawat
    %Background_BW=imgaussfilt(Background_BW,3);
    Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.1; 0.8], 3.0); 
   
    %Mean FG=204  to FG=210; %Threshold=60+Meandiffence2;
    %Mean FG=210  to FG=220; %Threshold=70+Meandiffence2;
        if Mean_FG<=160;
            Threshold=20+Meandiffence2;

            elseif Mean_FG<=170;
                   Threshold=30+Meandiffence2;

            elseif Mean_FG<=180;
                   Threshold=40+Meandiffence2;

            elseif Mean_FG<=190;
                   Threshold=50+Meandiffence2;

            elseif Mean_FG<=200;
                   Threshold=60+Meandiffence2;  

            elseif Mean_FG<=210;
                   Threshold=70+Meandiffence2;

            elseif Mean_FG<=220;
                   Threshold=80+Meandiffence2; 

            elseif Mean_FG<=230;
                   Threshold=90+Meandiffence2;

            elseif Mean_FG<=240;
                   Threshold=100+Meandiffence2; 

            elseif Mean_FG<=255;
                   Threshold=100+Meandiffence2;
        end

   elseif (Mean_BG < 230)%Pond
            Background_BW=imgaussfilt(Background_BW,5);
            %Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.1; 0.6], 3.0); 
            Threshold=90+Meandiffence2;
  elseif (Mean_BG < 240)%Jo
            %Background_BW=imgaussfilt(Background_BW,5);
            %Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.1; 0.6], 3.0); 
            Threshold=90+Meandiffence2;         
   elseif (Mean_BG < 250)%Mod
            Threshold=100+Meandiffence2;
else  

end

fprintf('Mean of Background: %0.2f \n',Mean_BG);
fprintf('Mean of Foreground: %0.2f \n',Mean_FG);
fprintf('Mean of Foreground difference1: %0.2f \n',Meandiffence1);
fprintf('Mean of Foreground difference2: %0.2f \n',Meandiffence2);
fprintf('PSNR: %0.2f \n',peaksnr);
fprintf('Threshold: %0.2f \n',Threshold);

clear BG;
clear Foreground;

%Getting the positions of the start point and the end point
%Calibration of distance between Marker M2 (Take-off board) and Marker M3 
%(5 maters from take-off board)

[PositionX,PositionY]=ginput(2);

%Getting the distance of the Approach-Run.

EndPoint = PositionX(1);
FiveMeters = PositionX(2)-PositionX(1);
TenMeters = FiveMeters*2;
StartPoint = EndPoint+TenMeters*3;

OneMeterW=(FiveMeters/5); %pixels per meter Width
OnePixelW=(5/FiveMeters); %meter per pixcel Width

MaxDistance=LongJumpW/(FiveMeters/5);
%Distance at the Start point to the End point = 20.5 meters convert to pixel format in the image.
DistanceStartToEnd = StartPoint-EndPoint;
RealTotalDistance = DistanceStartToEnd * OnePixelW;

%***************************************************************************************************************
%Loadding Foreground video
DurationTime = 10;% The value should be (4,8,10,12,16,20,24,32)
StepTime = 2;
%FrameRate = 60;
FrameRate=LongJump.FrameRate;
Velocity = 0;
PositionBody=0;

%Read and Starting at specific time
NumberofFrame= ceil(LongJump.FrameRate*LongJump.Duration); %Time frame of the vedio
Time = ceil(NumberofFrame/StepTime);

Index=1;
for count=1:StepTime:NumberofFrame-StepTime-DurationTime,    
      
    RealForeground=read(LongJump,count);
    RealForeground = imresize(RealForeground, 0.5);
    Foreground_BW = rgb2gray(RealForeground);
    
    %Adjust high to low  [low_in high_in],[low_out high_out],gamma)
    if (Mean_FG<145)%Teerawat01               
        Foreground_BW = imadjust(Foreground_BW,[0.00; 0.4],[0.5; 1.0], 0.3);
        
    elseif (Mean_FG<150)              
        Foreground_BW = imadjust(Foreground_BW,[0.00; 0.4],[0.5; 1.0], 0.3);   
    
    elseif (Mean_FG<155)              
        Foreground_BW = imadjust(Foreground_BW,[0.00; 0.4],[0.1; 0.3], 0.7);     
    
    elseif (Mean_FG<160)             
        Foreground_BW = imadjust(Foreground_BW,[0.0; 0.4],[0.1; 0.6], 0.5);       
     
    elseif (Mean_FG<170)
         Foreground_BW = imadjust( Foreground_BW,[0.0; 0.4],[0.1; 0.7], 0.6);
         
    elseif (Mean_FG<180)
         Foreground_BW = imadjust( Foreground_BW,[0.0; 0.4],[0.1; 0.8], 0.7);
    
    elseif (Mean_FG<190)
         Foreground_BW = imadjust( Foreground_BW,[0.0; 0.4],[0.1; 0.9], 0.9);
    
    elseif (Mean_FG<200)
         Foreground_BW = imadjust( Foreground_BW,[0.0; 0.4],[0.1; 0.8], 2.0);
         
    elseif (Mean_FG<215)
         Foreground_BW = imadjust( Foreground_BW,[0.0; 0.4],[0.1; 0.8], 2.0);    
    
    elseif (Mean_FG<225)
         %Background_BW = imadjust(Background_BW,[0.0; 0.4],[0.1; 0.6], 3.0);
         
    elseif (Mean_FG<230)
         %Foreground_BW = imadjust( Foreground_BW,[0.0; 0.4],[0.1; 0.6], 3.0);       
    
    elseif (Mean_FG<250)
         %Foreground_BW = imadjust( Foreground_BW,[0.0; 0.4],[0.1; 0.7], 1.0);      
    else
    end
              
    %Adjust Intensity Values Using Histogram Equalization
    Foreground_BW = imhistmatch(Foreground_BW, Background_BW);
    %imshow(Foreground_BW);
    ForegroundW= LongJump.Width*0.5;
    ForegroundH= LongJump.Height*0.5;
   
    %Subtraction Foregroung-Background
    SubtractionF_B1=imabsdiff(Foreground_BW,Background_BW);

    %Apply Median filter to remove Noise
    SubtractionF_B=medfilt2(SubtractionF_B1,[2 2]);
    %imshow(SubtractionF_B);
     
    
   %Filling black and white
    s=size(SubtractionF_B);
    y=zeros(s(1),s(2));
    for i=1:s(1)
            for j=1:s(2)
                if(SubtractionF_B(i,j)>Threshold)
                    FILL_BW(i,j)=0;
                else FILL_BW(i,j)=255;
                end
            end
    end

    [rows, columns]=size(FILL_BW);
    %Convert to Binary Image
    for i=1:rows
        for j=1:columns
            if FILL_BW(i,j) >0
                BinaryImage(i,j)=0;
            else
                BinaryImage(i,j)=1;
            end
        end
    end
    
    %Apply Median filter to remove Noise
    originalImage=medfilt2(BinaryImage,[2 2]);
               
    %Boundary Label the Filtered Image
    [L, num]=bwlabel(originalImage);
    STATS=regionprops(L,'all');
    cc=[];
    removed=0;
    
    %Remove the noisy regions 
    for i=1:num
        dd=STATS(i).Area;
        if (dd < PIXAREA)
            L(L==i)=0;
            removed = removed + 1;
            num=num-1;
        else
        end
    end
    
    [originalImage, num2]=bwlabel(L);
     %imshow(originalImage);
hold on;   

labeledImage = bwlabel(originalImage);
measurements = regionprops(labeledImage, 'BoundingBox', 'Area');

for k = 1 : length(measurements)
  thisBB = measurements(k).BoundingBox;
  rectangle('Position', [thisBB(1),thisBB(2),thisBB(3),thisBB(4)],...
  'EdgeColor','g','LineWidth',1 )
end

% Let's extract the second biggest blob - that will be the hand.
allAreas = [measurements.Area];
[sortedAreas, sortingIndexes] = sort(allAreas, 'descend');

try
Body = sortingIndexes(1); % The hand is the second biggest, face is biggest.

catch
 Body = 0;   
  
end

% Use ismember() to extact the hand from the labeled image.
Body = ismember(labeledImage, Body); 

siz=size(labeledImage);
CenterPoint=regionprops(labeledImage,'centroid');
centroids = cat(1, CenterPoint.Centroid);

try
BodyPosition = regionprops(originalImage,'Centroid');
BodyPositionX=BodyPosition.Centroid(:,1); %Body position of X
BodyPositionY=BodyPosition.Centroid(:,2); %Body position of Y

catch

        if (isempty(BodyPosition))
            BodyPositionX=MaxDistance;
            BodyPositionY=PositionX(1);        
        end    
end

%Caculating the distance of the body from the end point.
try
DistanceofBody = BodyPositionX-EndPoint;
catch
    break;
end

PositionofBody=DistanceofBody*OnePixelW;
BodyP(Index)=PositionofBody;
DistanceD(Index)=RealTotalDistance*(DistanceofBody/DistanceStartToEnd);
RealPositionBodyX(Index)=BodyPositionX;
RealPositionBodyY(Index)=BodyPositionY;
RealFrame(Index)=count;

%Create bounding Box
bb=regionprops(labeledImage,'BoundingBox');
 
%Crop the individual objects and store them in a cell
n=max(labeledImage(:)); % number of objects
ObjCell=cell(n,1);
for i=1:n
      % Get the bb of the i-th object and offest by 2 pixels in all
      % directions
      bb_i=ceil(bb(i).BoundingBox);
      idx_x=[bb_i(1)-10 bb_i(1)+bb_i(3)+10]; %  bb_i(1) read from 1st location
      idx_y=[bb_i(2)-10 bb_i(2)+bb_i(4)+10];
      if idx_x(1)<1, idx_x(1)=1; end
      if idx_y(1)<1, idx_y(1)=1; end
      if idx_x(2)>siz(2), idx_x(2)=siz(2); end
      if idx_y(2)>siz(1), idx_y(2)=siz(1); end
      % Crop the object and write to ObjCell
      im=labeledImage==i;
      ObjCell{i}=im(idx_y(1):idx_y(2),idx_x(1):idx_x(2));
end

%Plot in the pictures
 P=PositionY(1)*2;
 pos = [EndPoint*2  P;(EndPoint+FiveMeters)*2  P;(EndPoint+TenMeters)*2  P;StartPoint*2  P];
 color = {'green','green','green','green'};
 ForegroundM = insertMarker(RealForeground,pos,'x','color',color,'size',20); 
 ForegroundM = insertText(RealForeground, [BodyPositionX BodyPositionY], sprintf('%0.2f Meters',RealTotalDistance*(DistanceofBody/DistanceStartToEnd)));
 subplot(221);imshow(Background),title('Background');
 subplot(222);imshow(ForegroundM),title('Foreground');
 subplot(223);imshow(SubtractionF_B1),title('Background Subtraction');
 subplot(224);imshow(originalImage),title('Adaptive Background Subtraction');
 
  pause(0.01);
  hold on;
  plot(BodyPositionX,BodyPositionY, 'b*');
  plot(BodyPositionX,BodyPositionY, 'bo');
  %imshow(ObjCell{sortingIndexes(1)});
  hold off



Index=Index+1;
end

%Calculating velocity
for v=1:(Index-DurationTime-1),
P1_1=DistanceD(v);
P1_2=DistanceD(v+DurationTime);
rate=(1+(DurationTime*StepTime))*(1/FrameRate);
DistanceA= abs(P1_1-P1_2); 
velocityA(v)=DistanceA/rate;
PositionBody(v)=BodyP(v);

fprintf('Velocity at period %d is: %0.2f m/s. at (Distance=%0.3f,Position=%0.3f)\n',v,velocityA(v),DistanceA,PositionBody(v));
end

%**************************************************************************************************************************************************
%Define NaN replace the zero value
inx=1; 
err=0;
Error=1;
si=size(PositionBody);
        %Filtering of Distance position (NaN)
        for inx=1: max(si)-1
         NewPositionBody(inx) = PositionBody(inx);   
         NewvelocityA(inx) = velocityA(inx); 
         NewRealFrame(inx) = RealFrame(inx);
         NewRealPositionBodyX(inx) = RealPositionBodyX(inx);
         NewRealPositionBodyY(inx) = RealPositionBodyY(inx);
         Error(inx)=0;
         
         if(PositionBody(inx)<=-3)
         break;
         end
         
        if (velocityA(inx) > 13)
            NewPositionBody(inx) = NaN;
            NewvelocityA(inx) = NaN;
            Error(inx)=1;
        end
        
        if (PositionBody(inx)==0 || abs(PositionBody(inx+1)-PositionBody(inx)) >= 1.5)
            NewPositionBody(inx) = NaN;
            NewvelocityA(inx) = NaN;
            Error(inx)=1;
        end
         
         if (velocityA(inx)==0 || abs(velocityA(inx+1)-velocityA(inx)) >= 1.5 )
            NewPositionBody(inx) = NaN;
            NewvelocityA(inx) = NaN;
            Error(inx)=1;
         end
        
        inx=inx+1;
        end
        
% Percent of Data Error

ErrorSize=size(Error);
DataError= (sum(Error==1)/max(ErrorSize))*100;
      
%**************************************************************
%Raw Data
    try
        FillPositionBody = NewPositionBody;
        FillvelocityA = NewvelocityA;
    catch ME
            disp('An error occurred! Please change the background.');
            break;
    end

%**************************************************************
%Smooting data
x = FillPositionBody; 
y = FillvelocityA; 

FillPositionBody = smooth(x);
FillvelocityA = smooth(y);

%Detection of the startup position
take_inx=1;
start_inx=1;
for take_inx=1:(inx)-1;
    
        if (FillvelocityA(take_inx)>0)
            break;
        end
    
    start_inx=start_inx+1;
end

%Detection of the end position
take_inx=0;
speed_inx=0;
for take_inx=1:(inx)-1;
    
        if (FillPositionBody(take_inx)<=0)
            break;
        end
    
    speed_inx=speed_inx+1;
end

Startup_pos=FillPositionBody(start_inx);
End_pos=FillPositionBody(speed_inx);

Run_distance=Startup_pos;%-End_pos;
Runup_Time=(speed_inx*StepTime)/FrameRate;
Runup_Speed=Run_distance/Runup_Time;

%The result of startup position 
fprintf('Data Error(percent): %0.2f\n',DataError);  
fprintf('Run-up time(s): %0.2f \n',Runup_Time);
fprintf('Run-up speed(m/s): %0.2f \n',Runup_Speed);
fprintf('Run-up distance(m): %0.2f \n',Run_distance);

%Ploting velocity graph
plot_inx=1;
for i=start_inx:(speed_inx)-1,
 S(plot_inx)= FillPositionBody(i);
 V(plot_inx)= FillvelocityA(i);
 FinalRealFrame(plot_inx) = NewRealFrame(i);
 FinalRealPositionBodyX(plot_inx) = NewRealPositionBodyX(i);
 FinalRealPositionBodyY(plot_inx) = NewRealPositionBodyY(i);
 plot_inx=plot_inx+1;
end

%calculating Vmax
[M,I] = max(V);
V_Max= V(I);
PositionofV_Max=S(I);
fprintf('Maximum velocity is (m/s): %0.2f  \nDistance from the take-off board(m): %0.2f\n' ,V_Max,PositionofV_Max);
VmaxShow=read(LongJump,FinalRealFrame(I));
VmaxForeground=read(LongJump,FinalRealFrame(I));
VmaxForeground=insertMarker(VmaxForeground,pos,'x','color',color,'size',10); 
VmaxForeground=insertText(VmaxForeground, [FinalRealPositionBodyX(I)*2 FinalRealPositionBodyY(I)*2], sprintf('Vmax:(%0.2f m/s,%0.2f Meters)',V(I),S(I)),'FontSize',18);

%Rule of Phase I
if V_Max < 4
    Result_Run= sprintf(' Sorry your maximum velocity (Vmax): %0.2f m/s. that is not suitable for long jump. \n The maximum speed should be more than 4 m/s.',V_Max);        

    elseif PositionofV_Max==0 || PositionofV_Max<3
        Result_Run= sprintf(' Very Good: You are at the condition #1 \n Starting-position: %0.2f m. \n Maximum velocity (Vmax): %0.2f m/s. \n Maximum velocity position (Vmax position): %0.2f m.',Startup_pos,V_Max,PositionofV_Max);        
        
            elseif PositionofV_Max==0 || PositionofV_Max<5
            Result_Run= sprintf(' Very Good: You are at the condition #1 \n Starting-position: %0.2f m. \n Maximum velocity (Vmax): %0.2f m/s. \n Maximum velocity position (Vmax position): %0.2f m.',Startup_pos,V_Max,PositionofV_Max);        
                
                    elseif PositionofV_Max==5 || PositionofV_Max<10
                    Result_Run= sprintf(' Almost well: You are at the condition #2 \n Starting-position: %0.2f m. \n Maximum velocity (Vmax): %0.2f m/s. \n Maximum velocity position (Vmax position): %0.2f m.',Startup_pos,V_Max,PositionofV_Max);                
                     
                                elseif PositionofV_Max>=10 
                                Result_Run= sprintf(' Try again: You are at the condition #3 \n Starting-position: %0.2f m. \n Maximum velocity (Vmax): %0.2f m/s. \n Maximum velocity position (Vmax position): %0.2f m.',Startup_pos,V_Max,PositionofV_Max);   

end

VmaxForeground=insertText(VmaxForeground, [5 5], sprintf(Result_Run),'FontSize',18);
copyright=sprintf(' Knowledge-Based Smart Trainer for Long Jump Athletes Using Computer Vision Techniques \n Copyright 2019: Teerawat Kamnardsiri: College of Arts, Media and Technology, Chiang Mai University, Thailand.');
VmaxForeground=insertText(VmaxForeground, [5 680], sprintf(copyright),'FontSize',14);

%Saving the result of Vmax position of running and ploting distance and velocity.
figure1 = figure;
axes1 = axes('Parent',figure1);
hold(axes1,'all');
subplot(2,1,1);plot(smooth(S), smooth(V),'b-'),title('Velocity Graph');
% Add title and axis labels
xlabel('Distance (m)');
ylabel('Velocity (m/s)');
title('Relationship between Distance and Velocity');
saveas(figure1,[FileName,'_plot.jpg']);

figure2=figure;
axes2 = axes('Parent',figure2);
hold(axes2,'all');
imshow(VmaxForeground),title('Max Velocity (Vmax) of Running and Suggestion');
saveas(figure2,[FileName,'_result.jpg']);
  
%Saving the Running information 
save(fullfile([FileName,'.mat']),'V_Max','PositionofV_Max','DataError','Runup_Time','Runup_Speed','Run_distance','-ascii');
type(fullfile([FileName,'.mat']));

save(fullfile([FileName,'velocity.mat']),'S','V','-ascii');
type(fullfile([FileName,'velocity.mat']));