function [outP, outvc, outvs,vcon,vacc,an] = runcyclesimple(am,vm,tc,T,P,vc)
% an attempt at implementing the EMC simple_tp


% this is an ugly roundoff trick required
% T==P will never evaluate to true for some reason...
mul=1e6;
if round(T*mul)/mul == round(P*mul)/mul
    % done with this move
    
    % remove current segment from queue
    
    % get the next move
    
    % return 
    
    sprintf('done with segment')
    outP=T;
    outvc=0;
    outvs=0;
    vcon=0;
    vacc=0;
    an=0;
    return;
end

dist=(T-P);

% the negative of the distance to go, 
% reduced by the distance traveled during the next cycle if we have
% some vc right now and set vn=0
maxdv=am*tc;
vn=-maxdv+sqrt(2*am*dist+1*maxdv*maxdv)

if (vn<=0.1) 
    vn=0.1;
end    
vs=vn;  % store this vn in a variable vs = "v suggestion"

if (vn <= 0.0)
    sprintf('error, vn<=0.0, this should never happen !')
else
    % constrain velocity
    if vn>vm
        vn=vm;
        vcon=vn;  % store the constrained value for plotting
    else  
        % if velocity was not constrained, hide the vcon plot 
        vcon=-1;
    end
    
    % calculate acceleration
    an = (vn-vc)/tc;
    
    % constrain acceleration
    % and recalculate vn if neccessary
    if (an > 0.0) && (an > am)
        an= am;
        vn=vc+an*tc;
        vacc=vn;   % store for plotting
        vcon=-1; % if acceleration was constrained, hide the vcon plot
    elseif (an<0.0) && (an < (-am))
        an=- am;
        vn=vc+an*tc; 
        vacc=vn;    % store for plotting
        vcon=-1; % if acceleration was constrained, hide the vcon plot
    else
        vacc=-1; % if acceleration was not constrined, hide the vacc plot
    end    
    
    % update P
    % this math is simple enough, even I 'get it'
    P = P + (vn+vc)*0.5*tc;
end

vc    = vn;
outP  = P;
outvc = vc;
outvs = vs;