% preprocessing
% parse file
% calculate P0,V0,A0,J0 for all curves
% calculate curvature at all start and end points
% generate start and stop discontinuities

clear;
% *************************************************************************
% This section simulates parsing a file.  It just sets up an array of
% control points.  It specifies the number of subdivisions per curve.  The
% number of subdivions in the hardware design is 2^16.  This takes a long
% time to calculate.  We are using a smaller number in the simulation so
% that the run time is reasonable.
% *************************************************************************
P0x = [0   1   2];
P0y = [0   1   2];
P0z = [0   1   2];
P0e = [0   0   1];

P1x = [1   1   4];
P1y = [0   2   2];
P1z = [0   2   2];
P1e = [0   0   1];

P2x = [1   1   3];
P2y = [0   2   1.5];
P2z = [0   2   1.5];
P2e = [0   1   1];

P3x = [1   2   2];
P3y = [1   2   1];
P3z = [1   2   1];
P3e = [0   1   1];

pntsPerCurve = 2^16;
K    = 40000;    % this is used for finding the max velocity due to curvature
VxyzLimit=200;
ExtLimit =100;
VMin = 0.1;
HDSlope = 1000;

% *************************************************************************
% the next step is to put the control points into a form which is easier
% for the FPGA to compute.  By multiplying out the bezier polynomials and
% refactoring we can find new constants which will require less
% multiplications inside the FPGA.
%
% This is all done in preprocessing on the PC or within the program which
% creates the *.cbml file.  This code currently does not check for hard
% nonlinearities in the velocity.  It just checks the curvature.
% *************************************************************************

for n=1:length(P0x)
    
    % expand equations.  Solve for constant jerk form
    % reduces the number of multilies
    % speeds up loop
    % constant jerk equations of motion:
    % position = p0 + v0t + 1/2 a0t^2 + 1/6 j0t^3
    % velocity = v0 + a0t + 1/2 j0t^2
    % acceleration = a0 + j0t;
    % jerk = j0;
    
    % first derivative control points
    %dP0x(t)=3*(P1x(t)-P0x(t));
    %dP1x(t)=3*(P2x(t)-P1x(t));
    %dP2x(t)=3*(P3x(t)-P2x(t));
    % Velocity hodogram
    % dX  = (-t+1)^2*dP0x + (-t+1)*t*2*dP1x + t^2*dP2x;
    % dX  = (t^2-2t+1)*dP0x + (-2*t^2 + 2*t)*dP1x + t^2*dP2x;
    % dX  = (dP0x-2*dP1x+dP2x)*t^2 + (-2*dP0x + 2*dP1x)*t + dP0x;
    % desired form: velocity = v0 + a0t + 1/2 j0t^2
    % Vx0 = dP0x                        
    % Vx0 = 3*(P1x - P0x)
    
    % Ax0 = 2*dP1x-2*dP0x
    % Ax0 = 6*(P2x - P1x) - 6*(P1x - P0x))
    % Ax0 = 6*(P2x - 2*P1x + P0x)
    
    % Jx0/2 = dP0x-2*dP1x+dP2x
    % Jx0/2 = 3*(P1x-P0x) - 6*(P2x-P1x) + 3*(P3x-P2x)
    % Jx0/2 = 3*(P3x-P2x) - 2*3*(P2x-P1x) + 3*(P1x-P0x)
    % Jx0/2 = 3*(P3x - P2x - 2*P2x + 2*P1x + P1x - P0x)
    % Jx0 = 6*(P3x - 3*P2x + 3*P1x - P0x)
    
    % verify equation by using second hodogram
    % should have the same answers
    % second derivative control poitns
    % d2P0x(t)=2*(dP1x(t)-dP0x(t));
    % d2P1x(t)=2*(dP2x(t)-dP1x(t));
    % Acceleration hodogram
    % dX2 = (1-t)*d2P0x + t*d2P1x;
    % dX2 = (d2P1x - d2P0x)t + d2P0x
    % desired form: acceleration = a0 + j0t
    % Ax0 = d2P0x
    % Ax0 = 2 * (dP1x - dP0x)
    % Ax0 = 2 * (3*(P2x - P1x) - 3*(P1x - P0x))
    % Ax0 = 6P2x - 6P1x -6P1x + 6P0x
    % Ax0 = 6*(P2x + 2*P1x + P0x)
    
    % Jx0 = d2P1 - d2P0x
    % Jx0 = 2(dP2x-dP1x) - 2(dP1x-dP0x)
    % Jx0 = 2*(dP2x - dP1x -dP1x + dP0x)
    % Jx0 = 2*(dP2x - 2*dP1x + dP0x)
    % Jx0 = 2*(3*(P3x-P2x) - 2*3*(P2x-P1x) + 3*(P1x-P0x))
    % Jx0 = 6*(P3x - P2x - 2*P2x + 2*P1x + P1x - P0x)
    % Jx0 = 6*(P3x - 3*P2x + 3*P1x - P0x)
    
    Px0(n) = P0x(n);
    Vx0(n) = 3*(P1x(n) - P0x(n));
    Ax0(n) = 6*(P2x(n) - 2*P1x(n) + P0x(n));
    Jx0(n) = 6*(P3x(n) - 3*P2x(n) + 3*P1x(n) - P0x(n));
    
    Py0(n) = P0y(n);
    Vy0(n) = 3*(P1y(n) - P0y(n));
    Ay0(n) = 6*(P2y(n) - 2*P1y(n) + P0y(n));
    Jy0(n) = 6*(P3y(n) - 3*P2y(n) + 3*P1y(n) - P0y(n));
    
    Pz0(n) = P0z(n);
    Vz0(n) = 3*(P1z(n) - P0z(n));
    Az0(n) = 6*(P2z(n) - 2*P1z(n) + P0z(n));
    Jz0(n) = 6*(P3z(n) - 3*P2z(n) + 3*P1z(n) - P0z(n));

    Pe0(n) = P0e(n);
    Ve0(n) = 3*(P1e(n) - P0e(n));
    Ae0(n) = 6*(P2e(n) - 2*P1e(n) + P0e(n));
    Je0(n) = 6*(P3e(n) - 3*P2e(n) + 3*P1e(n) - P0e(n));

    % *********************************************************************
    % We also need the curvature at the endpoints of the arcs.  This is
    % used to set soft discontinuities in the motion profile.  These soft
    % discontinuites will create slow down and speed up areas near tight
    % corners.  There is are a few more cases which need to be considered
    % in the preprocessing.  These will not be performed in this system
    % simulation.  They will be explained later.  
    % Hard non linearities are handled by the program which writes the
    % *.cbml file.  
    % *********************************************************************
    % Curvature= 1/radius = Length(Cross(X'(t),X"(t)))/pow(Length(X'(t)),3)
    % CurveLimit = sqrt (Length(Cross(X'(t),X"(t)))/pow(Length(X'(t)),3))
    % V=sqrt(Vx0(n)^2+Vy0(n)^2+Vz0(n)^2);
    % Cross=sqrt((Vy*Az-Vz*Ay)^2 + (Vx*Az-Vz*Ax)^2 + (Vx*Ay-Vy*Ax)^2 );
    % CurveLimit=sqrt(K*V^3/Cross)
    
    % calculate velocity and curvature at start points
    Vx=Vx0(n);
    Vy=Vy0(n);
    Vz=Vz0(n);
    Ax=Ax0(n);
    Ay=Ay0(n);
    Az=Az0(n);
    V=sqrt(Vx^2+Vy^2+Vz^2);
    Theta=Vx/V;
    Phi=Vz/V;
    Cross=sqrt((Vy*Az-Vz*Ay)^2 + (Vx*Az-Vz*Ax)^2 + (Vx*Ay-Vy*Ax)^2 );
    StartCurveLimit(n)=sqrt(K*V^3/Cross);
    StartTheta(n)=Theta;
    StartPhi(n)=Phi;
    
    % calculate velocity and curvature at end points
    Vx  = Vx0(n) + Ax0(n) + Jx0(n)/2; 
    Vy  = Vy0(n) + Ay0(n) + Jy0(n)/2; 
    Vz  = Vz0(n) + Az0(n) + Jz0(n)/2; 
    Ax = (Ax0(n) + Jx0(n)); 
    Ay = (Ay0(n) + Jy0(n)); 
    Az = (Az0(n) + Jz0(n)); 
    V=sqrt(Vx^2+Vy^2+Vz^2);
    Theta=Vx/V;
    Phi=Vz/V;
    Cross=sqrt((Vy*Az-Vz*Ay)^2 + (Vx*Az-Vz*Ax)^2 + (Vx*Ay-Vy*Ax)^2 );
    EndCurveLimit(n)=sqrt(K*V^3/Cross);
    EndTheta(n)=Theta;
    EndPhi(n)=Phi;
    
    % calculate extrusion position and velocities at endpoints
    StartExt(n)    = Pe0(n);
    EndExt(n)      = Pe0(n)+ Ve0(n) + Ae0(n)/2 + Je0(n)/6;
    StartExtVel(n) = Ve0(n);
    EndExtVel(n)   = Ve0(n) + Ae0(n) + Je0(n)/2;
        
end

% *************************************************************************
% Here we use the calculated curvatures to specify the soft discontinuites 
% between arc segements.  The reduction in velocity is determined by the
% force created by the curvature.  The force is proportional to the
% velocity squared multiplied by the curvature.  The user specifies the
% maximum acceleration the machine can handle.  We set these equations
% equal to eachother and solve for the maximum velocity around the curve
% due to it's curvature.  We then set the minimum speed required at the
% discontinuity.  
% Small discontinuities may slip past this algorithm.  These should fall
% within the limits of the machine.  I don't think they will be a problem
% in the actual machine but might cause slight fluctionations in the
% velocity.
%
% A hard discontinuity is encountered when there is an instantanious change
% in the direction of the velocity during a transition from one curve to
% the next.  It is undesired and requires a complete stop at that point.
%
% Hard discontinuities can also be caused by an instantanious change in the
% extrusion or velocity of the extrusion from one curve to the next.  This
% will cause a full stop at the transition point.
% *************************************************************************

for n=1:length(StartCurveLimit)
    if(n==1) % very first arc.  Set hard discontinuity
        sdcntLimit(n) = VMin;
    elseif(     (StartExt(n)~=EndExt(n-1)) ||...
                (StartExtVel(n)~=EndExtVel(n-1)) ||...
                (StartTheta(n)~=EndTheta(n-1)) ||...
                (StartPhi(n)~=EndPhi(n-1)) )
        sdcntLimit(n) = VMin;    
    elseif(StartCurveLimit(n)<EndCurveLimit(n-1)) % Start Discontinuity Limited
        sdcntLimit(n) = StartCurveLimit(n);
    else % End Discontinuity from last arc Limited
        sdcntLimit(n) = EndCurveLimit(n-1);
    end
    edcntX(n) = Px0(n) + Vx0(n) + Ax0(n)/2 + Jx0(n)/6;
    edcntY(n) = Py0(n) + Vy0(n) + Ay0(n)/2 + Jy0(n)/6;
    edcntZ(n) = Pz0(n) + Vz0(n) + Az0(n)/2 + Jz0(n)/6;    
    if(n==length(EndCurveLimit)) % last arc.  Set discontinuity at end point
        edcntLimit(n) = VMin;
    elseif(     (StartExt(n+1)~=EndExt(n)) ||...
                (StartExtVel(n+1)~=EndExtVel(n)) ||...
                (StartTheta(n+1)~=EndTheta(n)) ||...
                (StartPhi(n+1)~=EndPhi(n)) )
        edcntLimit(n) = VMin;    
    elseif(EndCurveLimit(n)<StartCurveLimit(n+1))
        edcntLimit(n) = EndCurveLimit(n);
    else
        edcntLimit(n) = StartCurveLimit(n+1);
    end
end

% this code is designed to act similar to what will
% happen in the FPGA.
t=0;
pnt=1;
VStep=0;
VAccum=0;
while(t<length(P0x))
    intT=floor(t)+1;
    if(t==floor(t))
        Px=Px0(intT);
        Vx=Vx0(intT);
        Ax=Ax0(intT);
        Jx=Jx0(intT);
        
        Py=Py0(intT);
        Vy=Vy0(intT);
        Ay=Ay0(intT);
        Jy=Jy0(intT);
        
        Pz=Pz0(intT);
        Vz=Vz0(intT);
        Az=Az0(intT);
        Jz=Jz0(intT);

        Pe=Pe0(intT);
        Ve=Ve0(intT);
        Ae=Ae0(intT);
        Je=Je0(intT);

    else
        Px=Px+Vx/pntsPerCurve;
        Vx=Vx+Ax/pntsPerCurve;
        Ax=Ax+Jx/pntsPerCurve;
        
        Py=Py+Vy/pntsPerCurve;
        Vy=Vy+Ay/pntsPerCurve;
        Ay=Ay+Jy/pntsPerCurve;
        
        Pz=Pz+Vz/pntsPerCurve;
        Vz=Vz+Az/pntsPerCurve;
        Az=Az+Jz/pntsPerCurve;

        Pe=Pe+Ve/pntsPerCurve;
        Ve=Ve+Ae/pntsPerCurve;
        Ae=Ae+Je/pntsPerCurve;
    end
    %    Calculate speed profile variables
    V=sqrt(Vx^2+Vy^2+Vz^2);
    Cross=sqrt( (Vy*Az-Vz*Ay)^2 + (Vx*Az-Vz*Ax)^2 + (Vx*Ay-Vy*Ax)^2 );
    CurveLimit=sqrt(K*V^3/Cross);
    PeLimit=ExtLimit/Pe;
    % Curve Limit + slope * distance
    StartDiscLimit=sdcntLimit(intT) + HDSlope * ....
        sqrt( (Px0(intT)-Px)^2 + (Py0(intT)-Py)^2 + (Pz0(intT)-Pz)^2 );
    EndDiscLimit=edcntLimit(intT) + HDSlope * ....
        sqrt( (edcntX(intT)-Px)^2 + (edcntY(intT)-Py)^2 + (edcntZ(intT)-Pz)^2 );
     
    %   determine limiting factor
    if(CurveLimit<VxyzLimit)
        NextStep=CurveLimit;
    else
        NextStep=VxyzLimit;
    end
    if(NextStep>PeLimit)
        NextStep=PeLimit;
    end
    if(NextStep>StartDiscLimit)
        NextStep=StartDiscLimit;
    end
    if(NextStep>EndDiscLimit)
        NextStep=EndDiscLimit;
    end
    %accumulate and check velocity
    VAccum=VAccum + V/pntsPerCurve;
    if(VAccum>=VStep)
        VStep=VStep + NextStep/pntsPerCurve;
        RoughVelProfile(pnt)=NextStep/pntsPerCurve; %FIFO buffer rough profile
        pnt=pnt+1;
    end
    
    % step time
    t=t+1/pntsPerCurve;
end


% Upsampling CIC for rough profile
rate = 32;
CIC_filt = conv(conv(ones(1,rate),ones(1,rate)),ones(1,rate));
FiltVmax=conv(CIC_filt,RoughVelProfile)/rate^3;

% second pass
t=0;
pnt=1;
Vstep=0;
Vaccum=0;
PeAccum=0;
while(t<length(P0x))
    intT=floor(t)+1;
    fracT=t-floor(t);
    if(t==floor(t))
        Px=Px0(intT);
        Vx=Vx0(intT);
        Ax=Ax0(intT);
        Jx=Jx0(intT);
        
        Py=Py0(intT);
        Vy=Vy0(intT);
        Ay=Ay0(intT);
        Jy=Jy0(intT);
        
        Pz=Pz0(intT);
        Vz=Vz0(intT);
        Az=Az0(intT);
        Jz=Jz0(intT);

        Pe=Pe0(intT);
        Ve=Ve0(intT);
        Ae=Ae0(intT);
        Je=Je0(intT);
    else
        Px=Px+Vx/pntsPerCurve;
        Vx=Vx+Ax/pntsPerCurve;
        Ax=Ax+Jx/pntsPerCurve;
        
        Py=Py+Vy/pntsPerCurve;
        Vy=Vy+Ay/pntsPerCurve;
        Ay=Ay+Jy/pntsPerCurve;
        
        Pz=Pz+Vz/pntsPerCurve;
        Vz=Vz+Az/pntsPerCurve;
        Az=Az+Jz/pntsPerCurve;

        Ae=Ae+Je/pntsPerCurve;
        Ve=Ve+Ae/pntsPerCurve;
        Pe=Pe+Ve/pntsPerCurve;
    end
    Vaccum=Vaccum+sqrt(Vx^2+Vy^2+Vz^2)/pntsPerCurve;
    PeAccum=PeAccum + Pe*sqrt(Vx^2+Vy^2+Vz^2)/pntsPerCurve;
    while(Vaccum>Vstep)
        Px2Array(pnt)=Px;
        Py2Array(pnt)=Py;
        Pz2Array(pnt)=Pz;
        Pe2Array(pnt)=PeAccum;
        if(pnt<=length(FiltVmax))
            Vstep=Vstep+FiltVmax(pnt);
        else
            Vstep=100000;
        end
        pnt=pnt+1;
    end        
    t=t+1/pntsPerCurve;
end

dx=Px2Array(2:length(Px2Array))-Px2Array(1:length(Px2Array)-1);
dy=Py2Array(2:length(Py2Array))-Py2Array(1:length(Py2Array)-1);
dz=Pz2Array(2:length(Pz2Array))-Pz2Array(1:length(Pz2Array)-1);
for i=1:length(dx)
    TrueVelocity(i)=sqrt(dx(i)^2+dy(i)^2+dz(i)^2);
end
