%% Ønsker å generere en referanse som kobler sammen to punkter, a og b.
% Systemet skal start i ved a ved tiden t0, 
% så komme til ro vedb ved tiden t_f
%% Spesifikasjoner
a  = 1;
b  = 10;
t0 = 0;
tf = 60;

%% Finner koeffisientene
A=[  t0^3, t0^2,t0, 1; 
     tf^3, tf^2,tf, 1; 
   3*t0^2, 2*t0, 1, 0;
   3*tf^2, 2*tf, 1, 0;
   ];
B=[a;
   b;
   0;
   0;
   ];
x=A\B; % Samme som x=inv(A)*b
% alpha = x(1); beta = x(2); gamma = x(3); delta = x(4); 
disp(strcat(  "alpha=", num2str(x(1)),...
            ", beta=",  num2str(x(2)), ...
            ", delta=", num2str(x(3)), ...
            ", delta=", num2str(x(4))))
%% Lager referanse funksjon (kubiskPoly definert til slutt)
r = @(t) kubiskPoly(t,a,b,t0,tf,x);
% Plotter så denne:
t = linspace(0,100,1000);
refSig = zeros(length(t),1);
for i=1:length(t)
    refSig(i)=r(t(i));
end
figure(1);clf(1);
hold on;
plot(t,refSig);
plot([t(1),t(end)],[a,a],'r--');
plot([t(1),t(end)],[b,b],'g--');
ylim([a-0.1,b+0.1]);
legend({'$r(t)$','$a$','$b$'},'Interpreter','latex')
%% Kubisk polynom funksjon
function ref = kubiskPoly(t,a,b,t0,tf,params)    
    if t < t0
        ref = a;
        return;
    elseif t > tf
        ref = b;
        return;
    end
    ref = params(1)*t^3+params(2)*t^2+params(3)*t+params(4);    
end
