%% Speicher löschen
clear all;
clc;


%% Konfiguration 1
Ax=-100; 
Bx= 100;
Avorh=0;
Vab=10; %%Geschwindigkeit der Antriebe

%% Zähler Index
i=0;
%% Raster Schrittweite
step_x=10;
step_y=10;

%% Rasterbereich
x_min=-300;
x_max= 300;
y_min=   0;
y_max= 550;
d=((-x_min+x_max)/step_x)*((-y_min+y_max)/step_y); %%Anzahl Schleifendurchläufe

%% Antriebsbegrenzungen
Ay_min= -180;
Ay_max=  180;
By_min= -180;
By_max=  180;

%% Stablänge
r=360;

%% Schleife
h = waitbar(0,'Please wait...');         %% Initialisierung Waitbar
for TCPx = x_min : step_x : x_max        %% Raster x
           
       for TCPy = y_min : step_y : y_max %% Raster y
        i=i+1;
        b=i;
        [Ay(i),A2y(i),By(i),B2y(i)]=f_porty_arbeitsraum(TCPx,TCPy,r,Ax,Bx); %% Funktionsaufruf
        TCPX(i)=TCPx;                    %% Übergabe der TCP-Koordinaten an eine Matrix
        TCPY(i)=TCPy;
        rA=sqrt((Ax-TCPx).^2+(Ay-TCPy).^2);         %% Abstand vom TCP zum Antriebspunkt
        rA1=sqrt((Ax-TCPx).^2+(A2y-TCPy).^2);       %% Durch Fallunterscheidung erhält man 2 unterschiedliche Antriebspositionen
        rB=sqrt((Bx-TCPx).^2+(By-TCPy).^2);
        rB1=sqrt((Bx-TCPx).^2+(B2y-TCPy).^2);

            if Ay(i)<Ay_min | Ay(i)>Ay_max|r~=rA           %% Abfrage ob Antriebspunkte im erlaubten Bereich liegen
                if A2y(i)<Ay_min | A2y(i)>Ay_max|r~=rA1
                    TCPX(i)=nan;       
                    TCPY(i)=nan;
                    Ay(i)=nan;
                    A2y(i)=nan;
                end 
            end
            if B2y(i)<By_min | B2y(i)>By_max |r~=rB1
                if By(i)<By_min | By(i)>By_max|r~=rB
                     TCPX(i)=nan;       
                     TCPY(i)=nan;
                     By(i)=nan;
                     B2y(i)=nan;
                        
            end
          if Ay_max<Ay(i) | Ay(i)>Ay_min
             Ay=A2y;
          end
          if Ay_max<A2y(i) | A2y(i)>Ay_min
             Ay2=0;
          end
          if By_max<By(i) |By(i)>By_min
             By=B2y;
          end
          if By_max<B2y(i) |B2y(i)>By_min
             B2y=0;
          end
             
       if i>=2
                  j=i-1;
            TCP(i)=TCPX(i);
            
          xf(i)=TCPY(i)-TCPY(j);
          xg(i)=TCPY(1,i)-TCPY(1,i);
         [xa,xb,xc]=f_porty_geschwindigkeit2(TCPX,TCPY,Ay,By,i,j); %%Funktionsaufruf für Geschwindigkeitsverhältnisse
         xa(j)=xa;
         xb(j)=xb;
         xc(j)=xc;
         Vtcp(j)=xa(j)*Vab; %%Geschwindigkeit TCP
          Vtcpx(j)=xb(j)*Vab; %%Geschwindigkeit TCP in x-Richtung
          Vtcpy(j)=xc(j)*Vab; %%Geschwindigkeit TCP in y-Richtung
          
       end
    end
  end

waitbar(i/d)        
end
close(h) 
%% plot


figure(1);
set(gca,'FontSize',12);
title('Arbeitsraum Porty');
hold on;
axis([x_min x_max y_min y_max]);
plot (Ax,Ay_max, 'ro')
plot (Ax,Ay_min, 'ro')
plot (Bx,By_max, 'ro')
plot (Bx,By_min, 'ro')
plot (TCPX,TCPY)
xlabel('X-Axis in mm');
ylabel('Y-Axis in mm');
hold off;

figure(2);

patch([nan TCPX nan],[nan TCPY nan],[nan Vtcp nan], 'Edgecolor','flat');
hold off;