function [ ] = control(motion_config)

global do;
do = true;

% DH Parameter
a = [0,0,0,0,0,0,0];
alp = [-90,90,-90,90,-90,90,0];
d = [310.4,0,400.1,0,390,0,78];

links = 7;
L1 = Link('d',d(1),'a',a(1),'alpha',alp(1)/180*pi);
L1.isrevolute;
L2 = Link('d',d(2),'a',a(2),'alpha',alp(2)/180*pi);
L2.isrevolute;
L3 = Link('d',d(3),'a',a(3),'alpha',alp(3)/180*pi);
L3.isrevolute;
L4 = Link('d',d(4),'a',a(4),'alpha',alp(4)/180*pi);
L4.isrevolute;
L5 = Link('d',d(5),'a',a(5),'alpha',alp(5)/180*pi);
L5.isrevolute;
L6 = Link('d',d(6),'a',a(6),'alpha',alp(6)/180*pi);
L6.isrevolute;
L7 = Link('d',d(7),'a',a(7),'alpha',alp(7)/180*pi);
L7.isrevolute;
bot = SerialLink([L1 L2 L3 L4 L5 L6 L7], 'name', 'Kuka LBR4 +');


axes(motion_config.robofig)
% plot once because the axis get otherwise messed up
bot.plot();

while (motion_config.run)
    motion_config
    %Calculating 'M'
    
    % use this later for multiple points
    point =1;
    M_start = getM(motion_config.rx(point),   ...
                   motion_config.ry(point),   ...
                   motion_config.rz(point),   ...
                   motion_config.tx(point),   ...
                   motion_config.ty(point),   ...
                   motion_config.tz(point));
               
    M_end   = getM(motion_config.rx(point+1),   ...
                   motion_config.ry(point+1),   ...
                   motion_config.rz(point+1),   ...
                   motion_config.tx(point+1),   ...
                   motion_config.ty(point+1),   ...
                   motion_config.tz(point+1));
    
    args = [motion_config.arm,...
            motion_config.elbow,...
            motion_config.flip,...
            motion_config.delta(point)...
            ];
        
    for i=1:3
        if args(i) == 0
            args(i) = -1;
        end
    end
    
    animation_steps = ceil(motion_config.time/(motion_config.resolution/1000.0));
    t = linspace(0,1,animation_steps)';
    switch ( motion_config.type)
        case 'joint'
            % calculate start and end point
            try
                % calculate start joints 
                J_start = LBRinvKin(M_start,args,'LBR4+');

                % calculate end joints 
                J_end = LBRinvKin(M_end,args,'LBR4+');

            catch err
                err.message
            end
             
            J_animation = repmat(J_start,animation_steps,1);
            J_motion = J_end - J_start;
            
            % do the interpolation
            switch ( motion_config.core)
                case 'linear'
                % calculate joint movements   
                J_animation = repmat(linspace(0,1,animation_steps)',1,links);
                J_animation = J_animation.*repmat(J_motion,animation_steps,1); 
                [~,J_animation_d] = gradient( J_animation,t(2));
                [~,J_animation_dd] = gradient( J_animation_d,t(2));

                case 'bang bang'                    
                    [J_animation, J_animation_d, J_animation_dd] = mtraj(@bangbang, J_motion*0, J_motion, t);   
                case '5poly'
                    [J_animation, J_animation_d, J_animation_dd] = mtraj(@tpoly, J_motion*0, J_motion, t);   
                case 'lspb'
                    [J_animation, J_animation_d, J_animation_dd] = mtraj(@lspb, J_motion*0, J_motion, t);   
            end
            % add start joints
            J_animation = J_animation+repmat(J_start,animation_steps,1); 
            
            % calculate motion in 3d 

            

        case 'carthesian'
            switch ( motion_config.core)
                case 'linear'
                s = t;
                case 'bang bang'
                s = bangbang(0, 1, t);
                case '5poly'
                s = tpoly(0, 1, t);
                case 'lspb'
                s = lspb(0,1,t);
            end
            % generate a carthesian based line in space
            M_animation = zeros(4,4,animation_steps);
            
            for i = 1:animation_steps
                M_animation(:,:,i) = getM(...
                    motion_config.rx(point)+s(i)*(motion_config.rx(point+1)-motion_config.rx(point)),   ...
                    motion_config.ry(point)+s(i)*(motion_config.ry(point+1)-motion_config.ry(point)),   ...
                    motion_config.rz(point)+s(i)*(motion_config.rz(point+1)-motion_config.rz(point)),   ...
                    motion_config.tx(point)+s(i)*(motion_config.tx(point+1)-motion_config.tx(point)),   ...
                    motion_config.ty(point)+s(i)*(motion_config.ty(point+1)-motion_config.ty(point)),   ...
                    motion_config.tz(point)+s(i)*(motion_config.tz(point+1)-motion_config.tz(point))    ...
                    );
            end
            %M_animation = ctraj(M_start,M_end,s);                    

            J_animation = zeros(animation_steps,links);
            
            %setup the configuration of the current position
            arm = motion_config.arm;
            elbow = motion_config.elbow;
            flip = motion_config.flip;
            for i = 1:animation_steps
                try 
                    if (i == 1)
                        % set the starting point
                        J_animation(i,:) = LBRinvKin(M_animation(:,:,i),args,'LBR4+'); 
                    else
                        %not the starting point -> calculate by minimal
                        %change
                        [J_animation(i,:),new_arm,new_elbow,new_flip] = LBRinvKinMinChange(M_animation(:,:,i),J_animation(i-1,:),motion_config.delta(point),'LBR4+'); 
                        if (new_arm ~= arm || new_elbow ~= elbow || new_flip ~= flip )
                            
                            arm = new_arm;
                            elbow = new_elbow;
                            flip = new_flip;
                            fprintf('config change on %d  arm %d, elbow %d, flip %d \n',i,arm,elbow,flip);
                            
                        end
                    end
                        
                catch err
                    %err.message 
                    %fprintf('no join angle for animation_step %d use prev value instead\n',i);
                    if(i > 1)
                        J_animation(i,:) =  J_animation(i-1,:);
                    end
                end 
            end
            [~,J_animation_d] = gradient( J_animation,t(2));
            [~,J_animation_dd] = gradient( J_animation_d,t(2));
    end
    
    try
        
        % draw the joint speed and derivitives
        
        % plot the joint movement
        axes(motion_config.joinsfig)
        plot(t, J_animation);
        h = rotate3d;
        set(h,'RotateStyle','box','Enable','off');
        
        
        % plot the joint velocity
        axes(motion_config.joinsdfig)
        plot(t,J_animation_d);
        h = rotate3d;
        set(h,'RotateStyle','box','Enable','off');
        
        % plot the joint acceleration
        axes(motion_config.joinsddfig)
        plot(t,J_animation_dd);
        h = rotate3d;
        set(h,'RotateStyle','box','Enable','off');
        
        % do the animation
        axes(motion_config.robofig)
        h = rotate3d;
        set(h,'RotateStyle','box','Enable','on');

        tic % start timer
        
        acutual_animations = 0;
        while motion_config.run && toc <= motion_config.time
            i = toc / motion_config.time * animation_steps;
            i = ceil(i);
            if (i > animation_steps)
                i = animation_steps;
            end
            bot.plot(J_animation(i,:).*(pi/180));
            acutual_animations = acutual_animations +1 ;
        end
        if (acutual_animations < animation_steps)
            display(sprintf('warning skipped %d frames',animation_steps-acutual_animations));
        end
    catch err
        err.message
    end

end