%% Skript zum Plot mehrdimensionale Interpolation

%Skript für Plotbasics
basics_plot
figure
clf
% hold on
% box on
% grid on


P = [1 3 6 3 2;1 5 5 7 3;1 1 3 3 4];

mu = [0, 0.25, 0.5, 0.75, 1];


%% Polynomfit
%
ax = polyfit(mu,P(1,:),4);
ay = polyfit(mu,P(2,:),4);
az = polyfit(mu,P(3,:),4);

x_poly = polyval(ax,[0:0.01:1]);
y_poly = polyval(ay,[0:0.01:1]);
z_poly = polyval(az,[0:0.01:1]);

% plot([0:0.01:1],x_poly)

plot3(x_poly,y_poly,z_poly,'Color',myLineTwo);

hold on



plot3(P(1,:),P(2,:),P(3,:),'LineStyle','none','Marker','o','Color',myLineTwo,'MarkerEdgeColor','black','MarkerFaceColor','black')


text(P(1,1),P(2,1),P(3,1)-0.15,'$\mv{r}_0=\cp{0}$','Interpreter','none')
text(P(1,2),P(2,2),P(3,2)+0.12,'$\mv{r}_1$','Interpreter','none')
text(P(1,3)+0.08,P(2,3),P(3,3),'$\mv{r}_2$','Interpreter','none')
text(P(1,4),P(2,4),P(3,4)+0.12,'$\mv{r}_3$','Interpreter','none')
text(P(1,5),P(2,5),P(3,5)-0.20,'$\mv{r}_4=\cp{6}$','Interpreter','none')

axis equal


%%%%%%%%%%%%%%%%%%%%%%%%%%%
p = 3; % Man will für den Zwischenpunkt genau die Steigung noch angeben können. Evtl. muss man aber noch die zweite Ableitung mit angeben.
%%%%%%%%%%%%%%%%%%%%%%%%%%%

if mod(p,2) == 0    % Gerade
    subtr = 0;
else
    subtr = 1;
end


%% B*P = R
% Konstruktion des Vektors R in B*P = R
%size(Bahn(teilstueckIterator).pj,2) > 2*(Gi+1)  % Mehr als ein Punkt zu interpolieren
R = [P(:,1)';zeros(1,3);P(:,2:end-1)';zeros(1,3);P(:,end)'];

%% Berechnung der Basispolynomematrix B
%     Stetigkeit_Zwischenstueck = 1;
distribution_type = 'equally_spaced';
%       method = Gewünschte Methode zur Berechnung von u_par Es gibt 3
%                Methoden:
%         1. 'equally_spaced'
%             Gleiche Zeiteinheit je Verbindungsstrecke
%         2. 'cord_length_distribution'
%             Zeiteinteilung entsprechend des direkten Abstands der Punkte
%         3. 'centripetal_distribution'
%             Optimiert für scharfe Wendungen zwischen den Datenpunkten
% Berechnung des Parametervektors
u_par = berechne_u_par(P,distribution_type);  % Sollte bei zwei zu interpolierenden Punkten immer 0 und 1 sein.
% Berechnung des Knotenvektors
u = berechne_u(u_par, p);
Gi = 2;
subtr = 1;
grid on
% 
% 
% 
% uidx = [min(u):0.01:max(u)];
% 
% Ni2 = zeros(numel(uidx),numel(u)-1-2);
% Ni1 = zeros(numel(uidx),numel(u)-1-2);
% Ni0 = zeros(numel(uidx),numel(u)-1-2);
% 
% for i = 1:numel(uidx)
%     s = findspan(length(u)-p-2, p, uidx(i), u);
%     if s>4
%         stop = 1;
%     end
%     ndu = Allbasisfun(s,uidx(i),p,u);
%     Ni2(i,s-(p+1)+1:s) = ndu(:,end)';
% %     Ni1(i,s-(p+1)+1:s-1) = ndu(1:2,end-1)';
% %     Ni0(i,s-(p+1)+1:s-2) = ndu(1,end-2)';
% end
% 
% Ni2(Ni2==0)=nan;
% % Ni1(Ni1==0)=nan;
% % Ni0(Ni0==0)=nan;



% 
% for i = 1:size(Ni2,2)
%     
%     switch i
%         case 1
%             line=myLineOne;
% 
%         case 2
%             line=myLineTwo;
% 
%         case 3
%             line=myLineThree;
% 
%         case 4
%             line=myLineFour;
% 
%         case 5
%             line=myLineFive;
% 
%         case 6
%             line=myLineSix;
% 
%         case 7
%             line=myLineSeven;
% 
%         case 8
%             line=myLineOne;
% 
%     end
%     
%     %Punkte generieren:
%     
%     plot(uidx,Ni2(:,i),'LineStyle','-','Color',line)
%     axeshandle = findall(f2, 'type', 'axes');
%     set(axeshandle,'YLim',[0,1.1]);
%     
%     
% end

% legend('N_0','N_1','N_2','N_3','N_4','N_5','N_6','N_7')



B = zeros(2*(2+1)+3,5+p-1);      %Länge: 2*0-Gi-te Ableitung + alle zwischen Punkte (2- der Länge) x abhängig von gerades/ungerades p entweder n+1+p (p ger.) oder n+1+p-1 (p unger.)

for parIter = 1:length(u_par)
    if parIter == 1
        B((Gi+1)*(parIter-1)+1:(Gi+1)*parIter,parIter:parIter+p) = AllbasisfunDers(findspan(length(u)-p-2,p,u_par(parIter),u),u_par(parIter),p,u,2);
    elseif parIter == length(u_par)
        B(end:-1:end-Gi,parIter-1:parIter+p-1) = AllbasisfunDers(findspan(length(u)-p-2,p,u_par(parIter),u),u_par(parIter),p,u,2);
    else
        B((Gi+1)+(parIter-1):(0+1)+(parIter-1)+2,parIter:parIter+p) = AllbasisfunDers(findspan(length(u)-p-2,p,u_par(parIter),u),u_par(parIter),p,u,0);
    end
    
end

logvec = [true false true true true true true false true];

B = B(logvec,:);

%% Berechnung des Vektors P

P = B\R;

Pw = [P';ones(1,size(P',2))];

BahnBuff(1).pj = Pw;
BahnBuff(1).u = u;
BahnBuff(1).p = p;
%
vek = bspeval(BahnBuff(1).p,BahnBuff(1).pj,BahnBuff(1).u,0:0.01:1);
plot3(vek(1,:),vek(2,:),vek(3,:),'Color',myLineOne);
hold on
plot3(BahnBuff(1).pj(1,:)./BahnBuff(1).pj(end,:),BahnBuff(1).pj(2,:)./BahnBuff(1).pj(end,:),BahnBuff(1).pj(3,:)./BahnBuff(1).pj(end,:),'LineStyle','none','Marker','x','Color',myLineOne);
axis equal

plot3(BahnBuff.pj(1,:),BahnBuff.pj(2,:),BahnBuff.pj(3,:),'LineStyle','--','Color',myLineOne)

% for i = 1:3
%     vek = bspeval(BahnBuff(1).p,BahnBuff(1).pj,BahnBuff(1).u,0.25*i);
%     plot3(vek(1,:),vek(2,:),vek(3,:),'Color',myLineOne,'Marker','v','LineStyle','none');
% end

for k = 2:numel(BahnBuff.pj(1,1:end))-1
    text(BahnBuff.pj(1,k)./BahnBuff.pj(end,k)-0.05,BahnBuff.pj(2,k)./BahnBuff.pj(end,k)-0.05,BahnBuff.pj(3,k)./BahnBuff.pj(end,k)+0.08,['$\cp{' num2str(k-1) '}$'],'Interpreter','none')
end

set(gca,'XLim',[0,10])
set(gca,'YLim',[0,10])

xlabel('\figureXLabel')
ylabel('\figureYLabel')
zlabel('\figureZLabel')


matlab2tikz('filename','plot_mehrdimensionale_Interpolation.tex',...
    'height', '\figureheight', 'width', '\figurewidth', 'encoding', 'UTF8', 'showInfo', false, 'checkForUpdates', false, ...
    'figurehandle',gcf,...
    'parseStrings', false, ... % switch off LaTeX parsing by matlab2tikz for titles, axes labels etc. ("greater flexibility", "use straight LaTeX for your labels")
    'floatFormat', '%.4g', ... % limit precision to get smaller .tikz files
    'noSize', false);