function PlotBezier( bezier_points )
% Display the Bezier curve

% This goes in a file named PlotBezier.m

% Clear the children of the current axes

cla 

% Define the array of parametervectors as a column vector, so calculation
% of points on the Bezier curve can be done in one statement

t = (0:0.05:1)';

number_segments = size(bezier_points,1);
for k = 1:number_segments
    P0 = bezier_points{k,1};
    P1 = bezier_points{k,2};
    P2 = bezier_points{k,3};
    P3 = bezier_points{k,4};
    
    % Compute the points on this curve segment and plot them
    
    bezier = ((1 - t).^3)*P0 + (3*t.*(1 - t).^2)*P1 + (3*t.^2.*(1 - t))*P2 + t.^3*P3;
    plot(bezier(:,1),bezier(:,2))
    hold on
    
    % Plot dotted lines first so they will be behind the nodes and control
    % points and thus not intercept the clicks on the nodes and control
    % points
    
    plot([P0(1) P1(1)],[P0(2) P1(2)],':')
    plot([P2(1) P3(1)],[P2(2) P3(2)],':')
    
    % Plot the nodes and control points
    
    plot(P0(1),P0(2),'o','MarkerFaceColor','k','ButtonDownFcn',@ClickMarker)
     
    plot(P1(1),P1(2),'o','ButtonDownFcn',@ClickMarker)
    plot(P2(1),P2(2),'o','ButtonDownFcn',@ClickMarker)
end

% Plot the last node

plot(P3(1),P3(2),'o','MarkerFaceColor','k','ButtonDownFcn',@ClickMarker)

% Set the limits

xlim([0 1])
ylim([0 1])
function ClickMarker(source,event)
    set(ancestor(source,'figure'),'WindowButtonMotionFcn',{@DragMarker,source})
    set(ancestor(source,'figure'),'WindowButtonupFcn',@StopDragging)
end

function DragMarker(figure,event,source)

% Get current axes and coordinates of the screen point 

h1=gca;
coordinates=get(h1,'currentpoint');
coordinates = coordinates(1,1:2);

% Find the closest node or control point. This code is simplistic in the
% sense that it always takes the nearest node or control point. If a node
% or control point is dragged near another node or control point, then the
% other one will be moved.

segment_index = -1;
node_control_index = -1;
distance = Inf;
for m = 1:size(bezier_points,1)
    for n = 1:4
        if (isinf(distance))
            distance = norm(bezier_points{m,n}-coordinates);
            segment_index = m;
            node_control_index = n;
        else
            new_distance = norm(bezier_points{m,n}-coordinates);
            if (new_distance < distance)
                distance = new_distance;
                segment_index = m;
                node_control_index = n;
            end
        end
    end
end

% Modify the nodes and control points

old_point = bezier_points{segment_index,node_control_index};
bezier_points{segment_index,node_control_index} = coordinates;

% For an intermediate node, the coodinates are stored twice. Update the 
% other copy

if (segment_index < number_segments && node_control_index == 4)
    bezier_points{segment_index+1,1} = coordinates;
end

if (1 < segment_index && node_control_index == 1)
    bezier_points{segment_index-1,4} = coordinates;
end

% Moving a node or control point causes movement of other control points.
% If a node is moved, the control points on either side need to be moved. If a
% control point is moved, its partner acros the node must be moved

if (node_control_index == 1 || node_control_index == 4)
    
    % Node was moved. Move attached control points
    
    translation_vector = bezier_points{segment_index,node_control_index} - old_point;
    
    if (segment_index == 1 && node_control_index == 1)
        bezier_points{segment_index,2} = bezier_points{segment_index,2} + translation_vector;
    elseif (segment_index == number_segments && node_control_index == 4)
        bezier_points{segment_index,3} = bezier_points{segment_index,3} + translation_vector;
    else
        if (node_control_index == 1)
            bezier_points{segment_index,2} = bezier_points{segment_index,2} + translation_vector;
            bezier_points{segment_index-1,3} = bezier_points{segment_index-1,3} + translation_vector;
        else
            bezier_points{segment_index+1,2} = bezier_points{segment_index+1,2} + translation_vector;
            bezier_points{segment_index,3} = bezier_points{segment_index,3} + translation_vector;
        end
    end
else
    
    % Control point was moved. Rotate its partner
    
    if ((segment_index == 1 && node_control_index == 2) || (segment_index == number_segments && node_control_index == 3))
        
        % No partner. Nothing else to do
        
    else
        new_point = bezier_points{segment_index,node_control_index};
        if (node_control_index == 2)
            
            % Rotate previous control point
            
            rotation_center = bezier_points{segment_index,1};
            other_control_point = bezier_points{segment_index-1,3};
        else
            
            % Rotate next control point
            
            rotation_center = bezier_points{segment_index,4};
            other_control_point = bezier_points{segment_index+1,2};
        end
        
        new_vector = new_point - rotation_center;
        new_vector = new_vector/norm(new_vector);
        old_vector = old_point - rotation_center;
        old_vector = old_vector/norm(old_vector);

        cos_theta = sum(old_vector.*new_vector);
        sin_theta = old_vector(1)*new_vector(2) - new_vector(1)*old_vector(2);
        rotation_matrix = [cos_theta -sin_theta;sin_theta cos_theta];

        other_control_point = other_control_point - rotation_center;

        rotated_other_control_point = (rotation_matrix*(other_control_point'))' + rotation_center;
        
        if (node_control_index == 2)
            bezier_points{segment_index-1,3} = rotated_other_control_point;
        else
            bezier_points{segment_index+1,2} = rotated_other_control_point;
        end
       
    end
end

% Plot the updated curve

PlotBezier(bezier_points);

end
function StopDragging(figure,event)
    set(figure,'WindowButtonMotionFcn','')
    set(figure,'WindowButtonUpFcn','')
end

end