%****************************************************************
%        Hochschule Hamm-Lippstadt                              *
%****************************************************************
% Modul	          : BerechneSpielfeldausrichtung                *
%                                                               *
% Datum           : 24-Mai-2014                                 *
%                                                               *
% Funktion        : Die Ausrichtung der RoboSoccer Spielfeldes  *
%                   wird bestimmt.                              *
%                                                               *
% Implementation  : MATLAB R2013a                               *
%                                                               *
% Toolbox         : Image Processing, Computer Vision           *
%                                                               *
% Author          : Prof. Schneider                             *
%                                                               *
% Bemerkung       :                                             *
%                                                               *
% Letzte Änderung : 24. Mai 2014                                *
%                                                               *
%***************************************************************/
clc
close all
clear all

% Schalter um das Lade-GUI zu umgehen
bShortCut=true;
 
if (bShortCut==true)
    % alternative Bilddatei laden
    filename = 'Spielfeld_02.png';
    pathname = [cd,'\'];
    disp(['Alternatives Bild laden: ', fullfile(pathname, filename)])
else
    % Interaktiven Dialog starten
    [filename, pathname] = ...
        uigetfile({'*.png';'*.*'},'File Selector'); % Fokus auf Dateiendung '*.png'
    if isequal(filename,0)
        disp('User selected Cancel')  
    else
        disp(['User selected', fullfile(pathname, filename)])
    end
end
% Bild laden
Originalbild = imread([pathname, filename]);



% Bild in Graustufen wandeln
Grauwertbild = rgb2gray(Originalbild);

% Bildgröße bestimmen
[m,n] = size(Grauwertbild); % z.B. HxB = mxn = XxY 326x485 Pixel^2


% Kantenerkennung
Kantenbild = edge(Grauwertbild,'sobel');
figure
imshow(Kantenbild)
title('Sobel')


% Berechnung der Hough Transformation
[H,theta,rho] = hough(Kantenbild);

% Maxima im Houghraum suchen
P = houghpeaks(H,10,'threshold',ceil(0.4*max(H(:))));

% Linien im Bildraum generieren
lines = houghlines(Kantenbild,theta,rho,P,'FillGap',20,'MinLength',40);

% Geraden nahe Bildmittelpunkt wählen
% 1x10 struct array with fields:
%     point1
%     point2
%     theta
%     rho
nElements = 0;

% Randbereich des Bildes ausschließen
xSchwellwertMin = 10;
xSchwellwertMax = n-xSchwellwertMin;
ySchwellwertMin = xSchwellwertMin;
ySchwellwertMax = m-xSchwellwertMin;

% Spielfeldbreite in Pixel aus der Kameraperspektive
Spielfeld.Breite = 161;
Spielfeld.Laenge = 233;

% SpielfeldLänge in Pixel aus der Kameraperspektive

for i=1:length(lines)
    % Randlinien löschen
    if (lines(i).point1(1,1) < xSchwellwertMin) || (lines(i).point1(1,1) > xSchwellwertMax)||...
            (lines(i).point1(1,2) < ySchwellwertMin) || (lines(i).point1(1,2) > ySchwellwertMax)||...
            (lines(i).point2(1,1) < xSchwellwertMin) || (lines(i).point2(1,1) > xSchwellwertMax)||...
            (lines(i).point2(1,2) < ySchwellwertMin) || (lines(i).point2(1,2) > ySchwellwertMax)
        % Element löschen
        lines(i)=[];
    else
        lines(i).Richtungsvektor = [lines(i).point1(1,1)- lines(i).point2(1,1);  lines(i).point1(1,2)- lines(i).point2(1,2)];
        lines(i).n = [-lines(i).Richtungsvektor(2,1); lines(i).Richtungsvektor(1,1)];
        %lines(i).n0 = lines(i).n/norm(lines(i).n)
        if dot(lines(i).point2',lines(i).n)>=0
            lines(i).n0 = lines(i).n/norm(lines(i).n);
        else
            lines(i).n0 = -lines(i).n/norm(lines(i).n); % Normalenvektor weg vom Ursprung
        end;
        lines(i).d=dot(lines(i).n0,lines(i).point1');
    end;
end;
nErgebnis =0;
%% Alle Geraden miteinander vergleichen
% 2Do Brute Force das geht noch geschickter!
% noch einfacher: lines(i).theta vergleichen
for i=1:(length(lines)-1)
    for j=(i+1):length(lines)
        % Geraden in Normalenform
        Skalarprodukt = dot(lines(i).n0,lines(j).n0); % in deg 
        
        if abs(Skalarprodukt) > 0.99 % nahe 1, parallel
            % Winkel zur x-Achse in deg

            
            % Abstand der parallelen Geraden
            AbstandGeraden = dot(lines(j).point1',lines(i).n0)-lines(i).d;
            if abs(abs(AbstandGeraden)-Spielfeld.Breite)<10
                % Längsseite gefunden
                if (lines(i).d < lines(j).d)
                    Geraden.unten = j;
                    Geraden.oben = i;
                else
                    Geraden.unten = i;
                    Geraden.oben = j;
                end
            nErgebnis = nErgebnis+1;
            disp(['Ergebnis: Parallel sind die Geraden ',num2str(i),'->',num2str(j)])
            elseif abs(abs(AbstandGeraden)-Spielfeld.Laenge)<10
            if (lines(i).d < lines(j).d)
                    Geraden.links = i;
                    Geraden.rechts = j;
                else
                    Geraden.rechts = j;
                    Geraden.links = i;
                end    
            end
            nErgebnis = nErgebnis+1;
            disp(['Ergebnis: Parallel sind die Geraden ',num2str(i),'->',num2str(j)])
        end
    end;
end;

%% Schnittpunkt der Geraden

%% Schnittpunkt der Geraden bestimmen
% Ecke oben links
LambdaOL =  (lines(Geraden.oben).d-dot(lines(Geraden.links).point1',lines(Geraden.oben).n0))/dot(lines(Geraden.links).Richtungsvektor,lines(Geraden.oben).n0);
Schnittpunkt.OL = lines(Geraden.links).point1'+ LambdaOL*lines(Geraden.links).Richtungsvektor;

% Ecke oben rechts
LambdaOL =  (lines(Geraden.oben).d-dot(lines(Geraden.rechts).point1',lines(Geraden.oben).n0))/dot(lines(Geraden.rechts).Richtungsvektor,lines(Geraden.oben).n0);
Schnittpunkt.OR = lines(Geraden.rechts).point1'+ LambdaOL*lines(Geraden.rechts).Richtungsvektor;

% Ecke unten links
LambdaOL =  (lines(Geraden.unten).d-dot(lines(Geraden.links).point1',lines(Geraden.unten).n0))/dot(lines(Geraden.links).Richtungsvektor,lines(Geraden.unten).n0);
Schnittpunkt.UL = lines(Geraden.links).point1'+ LambdaOL*lines(Geraden.links).Richtungsvektor;

% Ecke unten rechts
LambdaOL =  (lines(Geraden.unten).d-dot(lines(Geraden.rechts).point1',lines(Geraden.unten).n0))/dot(lines(Geraden.rechts).Richtungsvektor,lines(Geraden.unten).n0);
Schnittpunkt.UR = lines(Geraden.rechts).point1'+ LambdaOL*lines(Geraden.rechts).Richtungsvektor;

%% Winkel berechnen
Winkel = lines(Geraden.links).theta;
disp(['Ausrichtungswinkel des Feldes: ',num2str(Winkel),'°'])


%% Torposition ermitteln
%Create a plot that superimposes the lines on the original image.

figure, imshow(Originalbild), hold on
max_len = 0;
for k = 1:length(lines)
   xy = [lines(k).point1; lines(k).point2];
   plot(xy(:,1),xy(:,2),'LineWidth',2,'Color','green');

   % Plot beginnings and ends of lines
   plot(xy(1,1),xy(1,2),'x','LineWidth',2,'Color','yellow');
   plot(xy(2,1),xy(2,2),'x','LineWidth',2,'Color','red');

   % Determine the endpoints of the longest line segment
   len = norm(lines(k).point1 - lines(k).point2);
   if ( len > max_len)
      max_len = len;
      xy_long = xy;
   end
   
   
end

% highlight the longest line segment
plot(xy_long(:,1),xy_long(:,2),'LineWidth',2,'Color','red');

% Ecken zeichnen
plot(Schnittpunkt.OL(1,1),Schnittpunkt.OL(2,1),'oy');
plot(Schnittpunkt.UL(1,1),Schnittpunkt.UL(2,1),'oy');
plot(Schnittpunkt.OR(1,1),Schnittpunkt.OR(2,1),'oy');
plot(Schnittpunkt.UR(1,1),Schnittpunkt.UR(2,1),'oy');

%% Spielfeld berechnen
Mittelpunkt = Schnittpunkt.OL + 0.5*(Schnittpunkt.OR - Schnittpunkt.OL)+0.5*(Schnittpunkt.UL-Schnittpunkt.OL);

%% Neue Figur mit Fußballfeld Overlay
figure, imshow(Originalbild), hold on
plot(Mittelpunkt(1,1),Mittelpunkt(2,1),'w.')

Radius = 40;
rectangle('Position',[Mittelpunkt(1,1)-Radius/2,Mittelpunkt(2,1)-Radius/2,Radius,Radius],'Curvature',[1,1],'LineWidth',2,'EdgeColor',[1,1,1])

% Linie
Mittellinie =[Schnittpunkt.OL + 0.5*(Schnittpunkt.OR - Schnittpunkt.OL),Schnittpunkt.OL + 0.5*(Schnittpunkt.OR - Schnittpunkt.OL)+(Schnittpunkt.UL-Schnittpunkt.OL)];
line(Mittellinie(1,:),Mittellinie(2,:),'LineWidth',2,'Color',[1,1,1])

% Tore
% rectangle('Position',[x,y,w,h])
Hoehe = 50;
Breite = 15;

LinkesTor = Schnittpunkt.OL+0.5*(Schnittpunkt.UL-Schnittpunkt.OL); % Eckposition
%hLinkesTor=rectangle('Position',[LinkesTor',Breite,Hoehe],'LineWidth',2,'EdgeColor',[1,1,1])
x=LinkesTor(1,1);
y=LinkesTor(2,1);
plot(x,y,'yo')
w=Breite;
h=Hoehe;
xv=[x-w/2 x+w/2 x+w/2 x-w/2 x-w/2];yv=[y-h/2 y-h/2 y+h/2 y+h/2 y-h/2];
%h=plot(xv,yv);axis equal;

%rotate angle alpha
R(1,:)=xv;R(2,:)=yv;
%alpha=30*2*pi/360;
alpha=Winkel;
% Translation (Finetuning)
 T(1,1:5)=0
 T(2,1:5)=3
KleinesRechteck=[cosd(alpha) -sind(alpha);sind(alpha) cosd(alpha)]*R+T;
hold on;plot(KleinesRechteck(1,:),KleinesRechteck(2,:),'w','LineWidth',2);

% Zweiter Kasten
w=Breite*2.5;
h=Hoehe*2;
xv=[x x+w x+w x x];yv=[y-h/2 y-h/2 y+h/2 y+h/2 y-h/2];
%h=plot(xv,yv);axis equal;

%% Abstoss
Abstoss = LinkesTor + 2*Breite*lines(Geraden.links).n0;
plot(Abstoss(1,1),Abstoss(2,1),'w.','LineWidth',2)

%rotate angle alpha
R(1,:)=xv;R(2,:)=yv;
% Translation (Finetuning)
 T(1,1:5)=-8
 T(2,1:5)=0
%alpha=30*2*pi/360;
alpha=Winkel;
GrossesRechteck=[cosd(alpha) -sind(alpha);sind(alpha) cosd(alpha)]*R+T;
hold on;plot(GrossesRechteck(1,:),GrossesRechteck(2,:),'w','LineWidth',2);

%% Kreissegment
R=15;
phid=60; % deg
n=0;
for phi=-phid+Winkel:1:phid+Winkel
    n=n+1; % Index
    KreisX(n)=Abstoss(1,1)+R*cosd(phi);
    KreisY(n)=Abstoss(2,1)+R*sind(phi);
end;
plot(KreisX,KreisY,'w-','LineWidth',2)

%% Rechtes Spielfeld
RechtesTor = Schnittpunkt.OR+0.5*(Schnittpunkt.UR-Schnittpunkt.OR); % Eckposition
plot(RechtesTor(1,1),RechtesTor(2,1),'yo')

%% Abstoss
Abstoss = RechtesTor - 2*Breite*lines(Geraden.links).n0;
plot(Abstoss(1,1),Abstoss(2,1),'w.','LineWidth',2)

% Translation (Finetuning)
T(1,1:5)=(Spielfeld.Laenge-Breite)*lines(Geraden.links).n0(1,1)
T(2,1:5)=(Spielfeld.Laenge-Breite)*lines(Geraden.links).n0(2,1)
XY=KleinesRechteck+T;
hold on;plot(XY(1,:),XY(2,:),'w','LineWidth',2);


% Translation (Finetuning)
T(1,1:5)=(Spielfeld.Laenge-w)*lines(Geraden.links).n0(1,1)
T(2,1:5)=(Spielfeld.Laenge-w)*lines(Geraden.links).n0(2,1)

alpha=Winkel;
XY2=GrossesRechteck+T;
hold on;plot(XY2(1,:),XY2(2,:),'w','LineWidth',2);


%% Kreissegment
R=15;
phid=60; % deg
n=0;
for phi=-phid+Winkel+180:1:phid+Winkel+180
    n=n+1; % Index
    KreisX(n)=Abstoss(1,1)+R*cosd(phi);
    KreisY(n)=Abstoss(2,1)+R*sind(phi);
end;
plot(KreisX,KreisY,'w-','LineWidth',2)