% Calcula uma trajectória polinomial de quinto grau, considerando
% velocidades inicial e final nulas.
%
% Syntax: traj=calctrajectory_poly3(t,pos0,posF,T)
%
% Parametros de entrada:
%   t    -> Array de instantes de tempo a calcular as posições previstas
%   pos0 -> Posição inicial da trajectória (x,y,z)
%   posF -> Posição final da trajectória (x,y,z)
%   T    -> Duração da trajectória
%
% Parâmetros de saída:
%   traj -> Array de posições da trajectória calculada para as três
%           dimensões (x,y,z)

function traj=calctrajectory_poly5(tlist,poslist,Tlist)

if (size(poslist,1)~=size(Tlist,1))
    error('2nd and 3rd arguments must have the same number of lines');
end
if (size(poslist,1)<2 || size(Tlist,1)<2)
    error('2nd and 3rd arguments must have at least 2 lines');
end

% Número de trajectos a efectuar
N=size(poslist,1);

% Caso a matriz Tlist só tenha uma coluna, usar durações máximas
if (size(Tlist,2)==1)
    Tlist=[Tlist zeros(N,1)];
    for i=1:N-1
        Tlist(i,2)=Tlist(i+1,1)-Tlist(i,1);
    end
    Tlist(end,2)=tlist(end)-Tlist(end,1);
end

% Inicializar a variável de saída
traj=zeros(length(tlist),size(poslist,2));

% Percorrer todas as dimensões
for j=1:size(poslist,2)

    % Antes das trajectórias
    tfind=find(tlist<Tlist(1,1));
    traj(tfind,j)=poslist(1,j);
    
    % Percorrer todas as posições
    for i=1:N-1
        
        % Posições limite
        pos0=poslist(i,j);
        posF=poslist(i+1,j);
        % Variação de posição
        delta_pos=posF-pos0;
        
        % Período da trajectória polinomial
        T=Tlist(i,2);
        
        % Intervalo de tempo máximo da trajectória a calcular
        delta_t=Tlist(i+1,1)-Tlist(i,1);
        % Instante de tempo inicial da trajectória
        t0=Tlist(i,1);
        % Instante de tempo final da trajectória polinomial
        if (delta_t<=T)
            tf=Tlist(i+1,1);
        else
            tf=t0+T;
        end
        
        % Amostras de t a utilizar
        tfind=find(tlist>=t0 & tlist<tf);
        % Intervalo tempo reinicializado
        t=tlist(tfind)-t0;
        
        % Coeficientes do polinómio de terceira ordem
        c0= pos0;
        c1= 0;
        c2= 0;
        c3= 10*delta_pos/T^3;
        c4=-15*delta_pos/T^4;
        c5=  6*delta_pos/T^5;

        % Trajectória prevista para esta dimensão
        pos=c0+c1*t+c2*t.^2+c3*t.^3+c4*t.^4+c5*t.^5;
        
        % Armazenamento do resultado
        traj(tfind,j)=pos;
        
        % Finalização da trajectória com um valor constante
        tfind2=find(tlist>=tf & tlist<Tlist(i+1,1));
        traj(tfind2,j)=posF;        
    end
    
    % Fim das trajectórias
    tfind=find(tlist>=Tlist(end,1));
    traj(tfind,j)=poslist(end,j);
end