classdef Trajectory < handle

%   Copyright 2017 The MathWorks, Inc.

    properties
       oldPosition;
       newPosition;
       direction;
       Vm;
       a;
       d;
       profile;
    end
    
    methods
        function this = Trajectory(oldPosition,newPosition,Vm,a,d)
        
            this.oldPosition = oldPosition;
            this.newPosition = newPosition;
            this.direction = sign(newPosition-oldPosition);
            this.Vm = Vm;
            this.a = a;
            this.d = d;
            
            x = abs(newPosition-oldPosition);
            
            
            
            term = 0.5*(Vm^2)*(1/a+1/d);
            
            if(x<100*eps)
               this.profile.type = 'null'; 
               this.profile.t1 = 0;
               this.profile.t2 = 0;
               this.profile.t3 = 0;
               this.profile.v = 0;
               this.profile.x = 0;
               return;
            elseif(x>term)
               this.profile.type = 'trapezoidal';
               this.profile.t1 = Vm/a;
               this.profile.t2 = this.profile.t1+(x-term)/Vm;
               this.profile.t3 = this.profile.t2+Vm/d;
               this.profile.v = Vm;
               this.profile.x = x;
               
               % sanity check
               area1 = 0.5*this.profile.t1*Vm;
               area2 = (this.profile.t2-this.profile.t1)*Vm;
               area3 = 0.5*(this.profile.t3-this.profile.t2)*Vm;
               area = area1+area2+area3;
               
               
            else
               this.profile.type = 'triangular';
               
               v = sqrt((2*x/(1/a+1/d)));
               this.profile.t1 = v/a;
               this.profile.t2 = this.profile.t1+0;
               this.profile.t3 = this.profile.t2+v/d;
               this.profile.v = v;                
               this.profile.x = x;
               
               % sanity check
               area = 0.5*this.profile.t3*v;
            end
            
           assert( (abs(x-area)/x)<100*eps);
            
        end
        
        function pos = getPosition(this,t)
            pos = getPositionAndVelocity(this,t);
        end
        function vel = getVelocity(this,t)
            [~,vel] = getPositionAndVelocity(this,t);
        end
        function [pos, vel,isAccel,isDecel] = getPositionAndVelocity(this,t)
            
            isAccel=false;
            isDecel=false;
            if(t<=0)
                vel = 0;
                pos = this.oldPosition;
                return;
            end
            if(t>=this.profile.t3)
                vel = 0;
                pos = this.newPosition;
                return;                
            end
            
            if(t<=this.profile.t1)                
                speed = this.a*t;
                dist = 0.5*speed*t;
                isAccel = true;
            elseif(t<=this.profile.t2)
                speed = this.profile.v;
                dist = 0.5*this.profile.v*this.profile.t1 + this.profile.v*(t-this.profile.t1);                
            else  %(t>this.profile.t3 && t<=this.profile.t3)
                speed = this.d*(this.profile.t3-t);
                dist = this.profile.x-0.5*speed*(this.profile.t3-t);
                isDecel = true;
            end
            vel = this.direction*speed;
            pos = this.oldPosition+this.direction*dist;        
        end
    end
    
    
end
