%% Funktion: Erstellung der Matrix für die Steinkoordinaten
% Praktikum Produktionstechnik
% WS 2021/2022
% Gruppe: Haris Adrovic, Maik Spinnrath, Franziska Troja
% Datum: 17.11.2021
% Letzte Änderung: 17.11.2021


function [position,Anzahl_Steine] = steinkoordinaten()
    steine = xlsread('Legosteine Positionierung.xlsx');    %Einlesen der Excel-Datei mit den Positionen der Steine und "Legoplatte"
    steinnummer = 0;                                            %Laufvariable fuer die Steine iniitiert
    [i,j] = size(steine);                                       %Groesse der "Legoplatte" auslesen
    steine_gefunden = true;                                     %Boolsche Variable fuer Abbruchbedingung

    while steine_gefunden == true                               %Wiederholen, solange Steine auf "Legoplatte" gefunden werden
        steinnummer = steinnummer + 1;                          %Steine nacheinander in der Matrix "steine" suchen
        for m = 1:i                                             %Zeilen durchgehen 
            for n = 1:j                                         %Spalten durchgehen 
               if steine(m,n) == steinnummer                    %Steinnummer an Position in Array Ueberpruefen, ob die sich dort befindet

                   % Stein liegt horizontal
                   if steine(m,n+2) == steinnummer              %Ueberpruefen, ob Stein horizontal liegt                  
                       %Mittelpunkt der Koordinate des Steins berechnen 
                       %-> fuer Weitergabe an Roboter zum Absetzen des Steins 
                       x = n + 1.5 - 2.5;                             
                       y = m + 0.5 - 2.5;                             
                       ausrichtung = 0; % Stein liegt horizontal, wenn ausrichtung = 0 

                       %Abspeicherung der Koordinaten und Ausrichtung des Stein
                       position(steinnummer,1) = x;
                       position(steinnummer,2) = -y;
                       position(steinnummer,3) = 0;
                       position(steinnummer,4) = ausrichtung;

                   % Stein liegt vertikal 
                   else                                         %Gegenereignis -> Stein muss vertikal liegen                
                       %Mittelpunkt der Koordinate des Steins berechnen 
                       %-> fuer Weitergabe an Roboter zum Absetzen des Steins 
                       x = n + 0.5 - 2.5;                             
                       y = m + 1.5 - 2.5;                             
                       ausrichtung = 1; % Stein liegt vertikal, wenn ausrichtung = 1

                       %Abspeicherung der Koordinaten und Ausrichtung des Stein
                       position(steinnummer,1) = x;
                       position(steinnummer,2) = -y;
                       position(steinnummer,3) = 0;
                       position(steinnummer,4) = ausrichtung;
                   end

                   break;
               end

               if steine(m,n) == steinnummer % for-Schleife fuer n abbrechen, wenn erste Koordinate der Steinnummer gefunden wurde
                   break
               end
            end

             if steine(m,n) == steinnummer % for-Schleife fuer m abbrechen, wenn erste Koordinate der Steinnummer gefunden wurde
                   break
             end
        end

        %am Ende der "Legoplatte" angekommen
        if m == i && n == j                                 
            Anzahl_Steine = steinnummer - 1;                 %Ausgabe der Anzahl der gefundenen Steine
            steine_gefunden = false;                        %Abbruchbedingung fuer while-Schleife
        end
    end

    %Ergaenzung der Matrix fuer Zuordnung der Magazine zu Legosteinnummerentnahme
    for i = 1:Anzahl_Steine
        %Magazin 1/rot
        if i >= 1 && i <= 4
            position(i,5) = 1;
        end

        %Magazin 2/gelb
        if i >= 5 && i <= 8
            position(i,5) = 2;
        end

        %Magazin 3/schwarz
        if i >= 9 && i <= 15
            position(i,5) = 3;
        end

        %Magazin 4/blau
        if i >= 16 && i <= 21
            position(i,5) = 4; 
        end
        
        %die übrigen schwarzen Steine müssen auf die anderen Magazine
        %verteilt werden
        if i >= 22 && i <= 24
            position(i,5) = 1;
        else 
            position(i,5) = 2;
        end
        
    end  
end