function [Tsoll,Xsoll,Ysoll,vXsoll,vYsoll,aXsoll,aYsoll]=Grobinterpolator(G_CODE,Tgrob)
% Funktion zur Erzeugung von Grobinterpolationspunkten aus den in der
% Matrix G_CODE abgelegten Verfahrsätzen   Zirn 27.9.04

Tsoll(1)=0; Xsoll(1)=0;Ysoll(1)=0; vXsoll(1)=0;vYsoll(1)=0; % Stillstand
vbahndef=G_CODE(1,4);   % Default-Bahngeschwindigkeit für Kreisbewegung
amaxdef=G_CODE(1,5);   % Default-Beschleunigung für Kreisbewegung
Elementezahl=size(G_CODE);      % Analyse der NC-Programmgröße
i=2;        % Grobinterpolationspunktindex
GGtrans=0;  % Merker für G01-G01 oder G01-G04-Übergang

% Satzweise Bearbeitung des NC-Programms in G_CODE
for k=2:1:Elementezahl(1)   % Abarbeitung der Programmzeilen
    %%%%%%%%%%%%%%%%%%
    %%% Geradeninterpolation  %%%
    %%%%%%%%%%%%%%%%%%
    if (G_CODE(k,1) == 1)   % G01
        % Längenermittlung des Satzes
        s=sqrt((G_CODE(k,2)-G_CODE(k-1,2))^2+(G_CODE(k,3)-G_CODE(k-1,3))^2);
        N=round(s/(Tgrob*G_CODE(k,4)));     % Anzahl Grobinterp.-Punkte im Satz
        dx=(G_CODE(k,2)-G_CODE(k-1,2))/N; % Schrittlänge zwischen den 
        dy=(G_CODE(k,3)-G_CODE(k-1,3))/N; % Grobinterpolationspunkten
        for n=1:1:N
            Tsoll(i)=i*Tgrob;    % Zeitbasis
            Xsoll(i)=Xsoll(i-1)+dx;% Sollpositon X-Achse
            Ysoll(i)=Ysoll(i-1)+dy;  % Sollpositon Y-Achse
            vXsoll(i)=dx/Tgrob; vYsoll(i)=dy/Tgrob; % Sollgeschwindigkeiten
            i=i+1;% Indexweiterschaltung
        end % of n
        vbahndef=G_CODE(k,4);   % Default-Bahngeschwindigkeit für Kreisbewegung
        amaxdef=G_CODE(k,5);   % Default-Beschleunigung für Kreisbewegung
    %%%%%%%%%%%%%%%%
    %%% Kreisinterpolation  %%%
    %%%%%%%%%%%%%%%%
    elseif (G_CODE(k,1) == 2)   % im Uhrzeigersinn G02
        % Berechnung der internen parametrischen Kreisdaten
        R=sqrt((G_CODE(k,4)-G_CODE(k-1,2))^2+(G_CODE(k,5)-G_CODE(k-1,3))^2);
        if (G_CODE(k-1,3) > G_CODE(k,5)) % Anfangswinkel
            betaA=acos((G_CODE(k-1,2)-G_CODE(k,4))/R);
        else
            betaA=2*pi-acos((G_CODE(k-1,2)-G_CODE(k,4))/R);
        end
        if (G_CODE(k,3) > G_CODE(k,5)) % Endwinkel
            betaE=acos((G_CODE(k,2)-G_CODE(k,4))/R);
        else
            betaE=2*pi-acos((G_CODE(k,2)-G_CODE(k,4))/R);
        end
        N=round(R*abs(betaE-betaA)/(Tgrob*vbahndef)); % Anzahl Grobinterp.-Punkte im Satz
        dbeta=abs(betaE-betaA)/N; % Schrittweite 
        omega=-dbeta/Tgrob;   % Winkelgeschwindigkeit
        for n=1:1:N
            Tsoll(i)=i*Tgrob;    % Zeitbasis
            Xsoll(i)=G_CODE(k,4)+R*cos(betaA-dbeta*n);% Sollpositonen
            Ysoll(i)=G_CODE(k,5)+R*sin(betaA-dbeta*n);  % Sollpositonen
            vXsoll(i)=-R*omega*sin(betaA-dbeta*n);
            vYsoll(i)=R*omega*cos(betaA-dbeta*n); % Sollgeschwindigkeiten
            i=i+1;% Indexweiterschaltung
        end % of n
    elseif (G_CODE(k,1) == 3)   % gegen den Uhrzeigersinn G03
        % Berechnung der internen parametrischen Kreisdaten
        R=sqrt((G_CODE(k,4)-G_CODE(k-1,2))^2+(G_CODE(k,5)-G_CODE(k-1,3))^2);
        if (G_CODE(k-1,3) > G_CODE(k,5)) % Anfangswinkel
            betaA=acos((G_CODE(k-1,2)-G_CODE(k,4))/R);
        else
            betaA=2*pi-acos((G_CODE(k-1,2)-G_CODE(k,4))/R);
        end
        if (G_CODE(k,3) > G_CODE(k,5)) % Endwinkel
            betaE=acos((G_CODE(k,2)-G_CODE(k,4))/R);
        else
            betaE=2*pi-acos((G_CODE(k,2)-G_CODE(k,4))/R);
        end
        N=round(R*abs(betaE-betaA)/(Tgrob*vbahndef)); % Anzahl Grobinterp.-Punkte im Satz
        dbeta=abs(betaE-betaA)/N; % Schrittweite 
        omega=dbeta/Tgrob;   % Winkelgeschwindigkeit
        for n=1:1:N
            Tsoll(i)=i*Tgrob;    % Zeitbasis
            Xsoll(i)=G_CODE(k,4)+R*cos(betaA+dbeta*n);% Sollpositonen
            Ysoll(i)=G_CODE(k,5)+R*sin(betaA+dbeta*n);  % Sollpositonen
            vXsoll(i)=-R*omega*sin(betaA+dbeta*n);
            vYsoll(i)=R*omega*cos(betaA+dbeta*n); % Sollgeschwindigkeiten
            i=i+1;% Indexweiterschaltung
        end % of n
    %%%%%%%%%%%%%%%%
    %%%        Wartezeit        %%%
    %%%%%%%%%%%%%%%%
    elseif (G_CODE(k,1) == 4)   % G04
        N=round(G_CODE(k,2)/Tgrob);  % Anzahl Grobinterp.-Punkte im Satz
        for n=1:1:N
            Tsoll(i)=i*Tgrob;    % Zeitbasis
            Xsoll(i)=G_CODE(k-1,2) ;% Sollposition X-Achse
            Ysoll(i)=G_CODE(k-1,3) ;  % Sollposition Y-Achse
            vXsoll(i)=0; vYsoll(i)=0; % Sollgeschwindigkeiten
            i=i+1;% Indexweiterschaltung
        end % of n
        G_CODE(k,2)=G_CODE(k-1,2); % Weiterreichung der Position vor G04
        G_CODE(k,3)=G_CODE(k-1,3); % zur Weiterverarbeitung z.B. für G02
    end % of if für G01,G02, G03 und G04-Unterscheidung
    
    % Moving-Average-Filterung der Punkte um den Satzübergang G01-G01 
    % und G04-G01
    w=round(vbahndef/(amaxdef*Tgrob));  % beschleunigungsabh. Filterbreite
    w=round(min(w,(N/3))); % Verrundungsbereich begrenzt auf 1/3 Satzlänge
    if (((i-(N+1)-w) >= 2)  && ((G_CODE(k,1) == 4) || (G_CODE(k,1) == 1)) && (GGtrans == 1)); 
        % Anfangswerte zu Bewegungsbeginn ausblenden und Überschleifen
        % nur bei G01/4 anwenden, wenn G01 oder G04 im vorherigen Satz
        for p=(i-(N+1)-w):1:(i-(N+1)+w) % Bearbeiten des Satzüberganges
            Xglatt(p)=Xsoll(p); Yglatt(p)=Ysoll(p); % Hilfswerte geglättet
            for q=1:1:w % Addition der umgebenden Sollwerte
                Xglatt(p)=Xglatt(p)+Xsoll(p-q)+Xsoll(p+q);
                Yglatt(p)=Yglatt(p)+Ysoll(p-q)+Ysoll(p+q);
            end % of q
            Xglatt(p)=Xglatt(p)/(2*w+1);  % gefilterte Hilfswerte durch
            Yglatt(p)=Yglatt(p)/(2*w+1); % Mittelwertbildung
        end % of p
        for p=(i-(N+1)-w):1:(i-(N+1)+w) % Übertragen der Hilfswerte als Sollwerte
            Xsoll(p)=Xglatt(p);% geglättete Sollwerte übertragen
            Ysoll(p)=Yglatt(p);%
            % ggf. nachträgliche Ruckfilterung über einen diskreten 
            % Tiefpass erster Ordnung mit Truck=amax/rmax 
            vXsoll(p)=(Xsoll(p)-Xsoll(p-1))/Tgrob; % gefilterte Geschwindigkeiten
            vYsoll(p)=(Ysoll(p)-Ysoll(p-1))/Tgrob;
        end % of p
    end % of if für die Satzübergangserkennung
    % Geraden-Geradenübergangsmerker anpassen
    if ((G_CODE(k,1) == 1) || (G_CODE(k,1) == 4))
        GGtrans=1;
    else 
        GGtrans=0;
    end
end % of Programmverarbeitung

% nachträgliche Beschleunigungsermittlung
aXsoll(1)=0;aYsoll(1)=0;
for i=2:1:length(Tsoll)
    aXsoll(i)=(vXsoll(i)-vXsoll(i-1))/Tgrob;
    aYsoll(i)=(vYsoll(i)-vYsoll(i-1))/Tgrob;
end

    