% % % % % % % % % % % % % % % %

% % Code for presentation

% % % % % % % % % % % % % % % %


n=1
while n>0
    for b=1:3
        jointAngle=moveManipulatorToHomePosition(jointAngle,joint1,joint2,joint3,joint4,joint5);
        % % % % % % % % % % % % % % % % %
        % Belt transports box into takeaway position
        % Check the box
        boxReadyForTakeaway = not( readDigitalPin(a,'D12') );
        % runBelt
        writeDigitalPin(a,'D8',1);
        % Check the box
        while not( boxReadyForTakeaway )
            boxReadyForTakeaway = not( readDigitalPin(a,'D12') );
        end
        % stopBelt
        writeDigitalPin(a,'D8',0);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % point1 -  above start box position
        jointAngle=moveManipulatorToBoxBeltReadyToTake(jointAngle, joint1,joint2,joint3,joint4,joint5)
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % point2 -  box position taken
        jointAngle=moveManipulatorToBoxBeltTake(jointAngle, joint1,joint2,joint3,joint4,joint5)
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % point3 -  Back to start box position
        jointAngle=moveManipulatorBackToBoxBeltReadyToTake(jointAngle, joint1,joint2,joint3,joint4,joint5)
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % point4 -  Go to color sensor
        jointAngle=moveManipulatorToColorSensorReadPosition(jointAngle, joint1,joint2,joint3,joint4,joint5)
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Check the color of box
        result = GetBoxColor (dev,WHITE,GREEN,BLACK,YELLOW,BLUE,RED)
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Move box with defined color to spot of the same color
        jointAngle=moveBoxToItsColorSpot(jointAngle, joint1,joint2,joint3,joint4,joint5,result,WHITE,GREEN,BLACK,YELLOW,BLUE,RED);
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Randomly define order to move boxes back
    
    boxReturnOrder = randperm(6)
    
    for i = 1:6
        n=boxReturnOrder(i)
        switch n
            % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Moving boxes back
            
            case 1
                % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % point1 -  blue back
                if jointAngle(10)==1
                    jointAngle= bringBlueBoxBackOnBelt(jointAngle, joint1,joint2,joint3,joint4,joint5)
                    writeDigitalPin(a,'D8',1)
                    pause(1)
                    writeDigitalPin(a,'D8',0)
                end
                % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % point2 -  green back
            case 2
                if jointAngle(6)==1
                    jointAngle=bringGreenBoxBackOnBelt(jointAngle, joint1,joint2,joint3,joint4,joint5)
                    writeDigitalPin(a,'D8',1)
                    pause(1)
                    writeDigitalPin(a,'D8',0)
                end
                % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % point3 -  White back
            case 3
                if jointAngle(9)==1
                    jointAngle=bringWhiteBoxBackOnBelt(jointAngle, joint1,joint2,joint3,joint4,joint5)
                    writeDigitalPin(a,'D8',1)
                    pause(1)
                    writeDigitalPin(a,'D8',0)
                end
                                % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % point3 -  Red back
                            case 4
                                
                if jointAngle(7)==1
                    jointAngle=bringRedBoxBackOnBelt(jointAngle, joint1,joint2,joint3,joint4,joint5)
                    writeDigitalPin(a,'D8',1)
                    pause(1)
                    writeDigitalPin(a,'D8',0)
       % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % point3 -  Yellow back
                end
            case 5
                if jointAngle(8)==1
                    jointAngle=bringYellowBoxBackOnBelt(jointAngle, joint1,joint2,joint3,joint4,joint5)
                    writeDigitalPin(a,'D8',1)
                    pause(1)
                    writeDigitalPin(a,'D8',0)
                end
                  % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % point3 -  Black back
            case 6
                if jointAngle(11)==1
                    jointAngle=bringBlackBoxBackOnBelt(jointAngle, joint1,joint2,joint3,joint4,joint5)
                    writeDigitalPin(a,'D8',1)
                    pause(1)
                    writeDigitalPin(a,'D8',0)
                end
        end
    end
    % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % point last -  go to home position
    jointAngle=moveManipulatorToHomePosition(jointAngle,joint1,joint2,joint3,joint4,joint5);
end