function [s,v,a,k1,k2,n] = s_prof(vmax,amaxp,amaxm,alfa,deltat)

% Uso: [s,v,a,k1,k2,n] = s_prof(vmax,amaxp,amaxm,alfa,deltat)
%
% Calcola i valori discreti del profilo s(k) di tipo 2-1-2
% la velocita` v(k) e l'accelerazione a(k)
% con i vincoli di velocita` massima = vmax
% accelerazione massima = amaxp
% decelerazione massima = amaxm
%
% alfa = percentuale di velocita` massima effettivamente applicata
% deltat = periodo di campionamento
% k1,k2 = valori dei campioni di commutazione
% n = numero totale di campioni s(k), k=1:n

vmax = alfa*vmax;
k = 1;
s(k)=0.0;
v(k)=0.0;
a(k)=amaxp;
iflag = 1;	% primo tratto accelerazione costante
while s(k) < 1.0, 
	if iflag == 1
		sr = 1-s(k);
		vp = v(k)+amaxp*deltat;
		if(sr > (v(k)*deltat+0.5*amaxp*deltat^2+0.5*vp^2/amaxm)) 
			% ho ancora spazio per un altro passo a velocita` crescente
			v(k+1) = vp;
			if(v(k+1) > vmax)
				% passo a velocita` costante
				v(k+1) = vmax;
				a(k+1) = (vmax-v(k))/deltat;
				s(k+1) = s(k) + v(k)*deltat + 0.5*(vmax-v(k))*deltat;
				iflag = 2;
			else
				% passo a accelerazione costante
				a(k+1) = amaxp;
				s(k+1) = s(k) + v(k)*deltat + 0.5*amaxp*deltat^2;
			end
			k1 = k;
			k = k+1;
		else
			% inizio a decelerare
			c2=deltat^2;
			c1=-(amaxm*deltat^2+2*v(k)*deltat);
			c0=v(k)^2-2*amaxm+2*amaxm*s(k)+2*amaxm*v(k)*deltat;
			r=roots([c2 c1 c0]);
			ad=min(r);
			a(k+1) = -ad;
			v(k+1) = v(k) - ad*deltat;
			s(k+1) = s(k) + v(k)*deltat - 0.5*ad*deltat^2;
			iflag = 3;
			k2 = k;
			k = k+1;
		end
	elseif iflag == 2 % secondo tratto velocita costante
		sr = 1-s(k);
		if(sr > (vmax*deltat+0.5*v(k)^2/amaxm)) 
			% ho ancora spazio per un altro passo a max velocita`
			a(k+1) = 0;
			v(k+1) = vmax;
			s(k+1) = s(k) + v(k)*deltat;
		else
			% inizio a decelerare
			c2=deltat^2;
			c1=-(amaxm*deltat^2+2*v(k)*deltat);
			c0=v(k)^2-2*amaxm+2*amaxm*s(k)+2*amaxm*v(k)*deltat;
			r=roots([c2 c1 c0]);
			ad=min(r);
			a(k+1) = -ad;
			v(k+1) = v(k) - ad*deltat;
			s(k+1) = s(k) + v(k)*deltat - 0.5*ad*deltat^2;
			iflag = 3;
		end
		k2 = k;
		k = k+1;
	elseif iflag == 3 % terzo tratto decelerazione costante
		vk2 = v(k) - 2*amaxm*deltat; % calcolo vel due passi in avanti
		if(vk2 > 0) % mi sta bene e continuo a decelerare
			a(k+1) = -amaxm;
			v(k+1) = v(k) - amaxm*deltat;
			s(k+1) = s(k) + v(k)*deltat - 0.5*amaxm*deltat^2;
			k=k+1;
		else	% devo evitare di andare a vel negativa
			a1f = (s(k)+1.5*v(k)*deltat-1)/deltat^2;
			a2f = (1-s(k)-0.5*v(k)*deltat)/deltat^2;
			if((a1f > amaxm)|(a2f > amaxm))
				a(k+1) = -amaxm;
				a(k+2) = -amaxm;
				v(k+1) = v(k) - a1f*deltat;
				v(k+2) = v(k+1) - a2f*deltat;
				s(k+1) = s(k) + v(k)*deltat - 0.5*a1f*deltat^2;
				s(k+2) = s(k+1) + v(k+1)*deltat - 0.5*a2f*deltat^2;
				k = k+2;
			else
				a(k+1) = -a1f;
				a(k+2) = -a2f;
				v(k+1) = v(k) - a1f*deltat;
				v(k+2) = v(k+1) - a2f*deltat;
				s(k+1) = s(k) + v(k)*deltat - 0.5*a1f*deltat^2;
				s(k+2) = s(k+1) + v(k+1)*deltat - 0.5*a2f*deltat^2;
				k = k+2;
			end
		end
	end	
n = k;
end

return


