function drawBridge(bridgeStruct, loadVec, internalForces)
% drawBridge(bridgeStruct)
% drawBridge(bridgeStruct, loadVec)
% drawBridge(bridgeStruct, loadVec, internalForces)
%
% draws bridge in the bridge struct
%
% from Ax = b:
%
% optionally draws load weights in the 4n x 1 loadVec vector (b in the
% equation)
% 
% optionally draws the internal forces in the 4n x 1 internalForces vector
% (x in the equation)

    
    n = bridgeStruct.sections;
    if nargin > 1
        if max(size(loadVec) ~= [4*n 1]) > 0
            error(['Wrong size on load vec: ' num2str(size(loadVec))]);
        end
        if nargin > 2
            if max(size(internalForces) ~= [4*n 1]) > 0
                error(['Wrong size on internal forces: ' num2str(size(internalForces))]);
            end            
        end
    end

    numJoints = bridgeStruct.numJoints;
    numMembers = bridgeStruct.numMembers;
    span = bridgeStruct.span;
    height = bridgeStruct.height;

    for i = 1 : numJoints
        jointPos = getJointPos(bridgeStruct, i);
        plot(jointPos(1), jointPos(2), 'k.');
        hold on;
    end

    for i = 1 : numMembers
        [from to] = getMemberPos(bridgeStruct, i);
        plot([from(1) to(1)], [from(2) to(2)], 'k:');
    end

    sampleLoadForce = 0;
    lengthPerNewton = 0;
    if nargin > 1 %draw loads
        maxForce = max(abs(loadVec));
        sampleLoadForce = maxForce;
        if(nargin > 2)
            maxForce = max(maxForce, max(abs(internalForces)));
        end
        sampleLength = span / bridgeStruct.sections / 2.2;
        lengthPerNewton =  sampleLength / maxForce;
        arrowSize = span / 120;
    end

    axis equal;

    border = .15 * min(span, height);
    axis([-border, span + border, -border - lengthPerNewton * sampleLoadForce, height + border]);

    if nargin > 1 %draw loads
        for i = 1 : numJoints
            jointLoad = loadVec(i * 2 - 1 : i * 2)';
            if(max(abs(jointLoad)) > 0)
                drawForce(i, -jointLoad, 'k');
                
%                 from = getJointPos(bridgeStruct, i);
%                 to = from - jointLoad * lengthPerNewton;
%                 arrow3(from, to, '', arrowSize, arrowSize);
            end
        end        
    end


    if nargin > 2 %draw internal forces    
        for i = 1 : numMembers
            memberForce = internalForces(i);
            [from to] = getMemberPos(bridgeStruct, i);
            if(memberForce > 0)
                color = 'b';
            else
                color = 'r';
            end                

            forceStr = num2str(abs(memberForce), 3);
            xText = (from(1) + to(1))/2;
            text(xText, (from(2) + to(2))/2, forceStr, 'Color', color, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom');
            if abs(memberForce) > 0
                fromArrow = [from; to];
                %fromArrow = from;
                dir = (to - from) / norm(to - from);
                arrowLength = abs(memberForce) * lengthPerNewton;

                toArrow(1, 1:2) = from + arrowLength * dir;
                toArrow(2, 1:2) = to - arrowLength * dir;
                if(memberForce > 0)
                   [fromArrow, toArrow] = swap(fromArrow, toArrow); 
                end                
                arrow3(fromArrow, toArrow, color, arrowSize, arrowSize);
            end
        end        
        %draw supporting forces
        drawForce(1, [0 internalForces(numMembers + 1)], 'g');
        drawForce(numJoints, [0 internalForces(numMembers + 2)], 'g');
        drawForce(numJoints, [0 internalForces(numMembers + 2)], 'g');
        drawForce(1, [internalForces(numMembers + 3), 0], 'g');
    end


    set(gca,'xtick',[0 span])
    set(gca,'ytick',[0 height])

    function drawForce(jointNum, force, color)        
        if norm(force) > 0
           jointPos = getJointPos(bridgeStruct, jointNum);
           endArrow = jointPos + force * lengthPerNewton;
           arrow3(jointPos, endArrow, color, arrowSize, arrowSize);
           alignment = 'bottom';
           if force(2) < 0
               alignment = 'top';
           end
           text(endArrow(1), endArrow(2), num2str(norm(force), 3), 'Color', color, 'HorizontalAlignment', 'center', 'VerticalAlignment', alignment);
        end
    end
end

function [c d] = swap(a, b)
    c = b;
    d = a;
end

function [from to] = getMemberPos(bridge, memberNum)
    [fromInd toInd] = getMemberJoints(bridge, memberNum);
    from = getJointPos(bridge, fromInd);
    to = getJointPos(bridge, toInd);
end


