function [] = robot_build(defined_color,o_width,o_length,floor,ur,ser,TextArea,Gauge)
% Popis funkcie:
% 
%% Fixne pozicie
default_position = [0.0313   -0.3786    0.2128   3.1412    0.0508    0.0000];   % vychodzia pozicia robota
start_position = [-0.1730   -0.609    0.0445   2.2016    2.2411   -0.0000];     % uchopovacia pozicia(pri snimacoch)
position_1 = start_position; position_1(3) = position_1(3) + 0.015;             % pozicia nad uchopovaciou pouziciou
measure_position_1_1 = [0.092   -0.5745    0.0363    3.1415    0.0179   -0.0000];   % pozicia na polozenie kocky k meraniu
measure_position_1_2 = [0.109   -0.5737    0.0363    3.1415    0.0179   -0.0000];   % pozicia k uchyteniu poslednej kocky
measure_position_1 = [0.101   -0.574    0.0365    3.1415    0.0179   -0.0000];      % pozicia k uchopeniu kocky priecne po merani
position_above_measure_1 = measure_position_1; position_above_measure_1(3) = position_above_measure_1(3) + 0.028;     % pozicia nad priecnym uchopenim
measure_position_2 = [0.1020   -0.5751    0.0365    2.2071    2.2357   -0.0000];    % pozicia s otocenim na pozdlzne meranie kocky/pozdlzne uchopenie 
position_above_measure_2 = measure_position_2; position_above_measure_2(3) = position_above_measure_2(3) + 0.028;     % pozicia nad pozdlznym uchopenim
first_position_above_end = [0.150   -0.422    0.050   -3.1414   -0.0290    0.0000];    % zaciatocna pozicia k postaveniu 1. kocky
%% Definicia objektu
length_of_small_block = 0.0161; % velkost malej kocky
index_of_block = 0;             % pociatocny index bloku
previous_move = 0;              % predchadzajuci pohyb
last_block = 0;                 % indikacia posledneho bloku
big_grip = 0;                   % pozdlzne uchopovanie je deaktivovane
rotation_after_big_grip = 0;    % rotacia po pozdlznom uchope
correction = 1;                 % nastavenie prvej korekcie smerom do zaporu osi x
correction_depth = [0.0339];    % pociatocna hlbka pre zasuvanie kociek s korekciou
%% Prepocet pozicii objektu
defined_length_of_object = 2 * (o_width + o_length) - 4;        % celkova dlzka 1 poschodia objektu
corner_index_big_grip(1) = defined_length_of_object  - (o_width-2); % index 1. rohu pre pozdlzne uchopenie
corner_index_big_grip(2) = corner_index_big_grip(1) - (o_length-1);
corner_index_big_grip(3) = corner_index_big_grip(2)- (o_width-1);
corner_index_normal = corner_index_big_grip - 1;        % indexy rohov bez uchytu pozdlzne(normlany prechod)
for j = 2:max(floor)                                    % prepocet hlbok pre zasuvanie kocky s korekciou
    correction_depth(j) = correction_depth(j-1) + 0.00965;      % posun hlbky korekcie vzdy o vysku kocky
end
%% Instrukcie k pohybu robota
ur.movej(default_position);     % najkratsi pohyb do vychodzej pozicie robota
ur.RG2(40,30,0,false,false);    % rozovretie gripperu na sirku 40 mm so silou 30 N
k = 1;                          % premenna pre osetrenie zaseknutia vo while
while ur.gripper_pose < 20
     ur.RG2(40,30,0,false,false);% opakovanie rozovretia gripperu
     if k > 5 ;return; end      % ak sa zasekne v smycke po 5 opakovaniach sa program prerusi
     k = k + 1;                 % inkrementacia premmenej
end
k = 1;
for floor_number = 1:max(floor)         % stavanie nastaveneho poctu poschodi
        iteration = 0;                  % index prveho zasuvania aby zacal na rovnakej pozicii
        move = 1;                       % zaciatocny pohyb dolava
        length_of_object = defined_length_of_object;
        position_above_end = first_position_above_end;  % zaciatocna pozicia fixna pre kazde poschodie
        if floor_number > 1        % zdvihnutie pociatocnej polohy pri 2 a viac poschodiach
            position_above_end(3) = position_above_end(3) + (floor_number/2 * 0.015);
        else                       % 1. poschodie bez zmeny
            position_above_end(3) = position_above_end(3);
        end
    while length_of_object > 0     % dokym nieje dlzka 1 poschodia objektu rovna 0 
        ur.movej(position_1);        
        [color_number,distance] = read_sensors(ser);    % nacitanie farby a vzdialenosti zo serial portu(arduino)

        if distance < 410  
                ur.movel(start_position);
            if (color_number == defined_color(floor_number)) || (5 == defined_color(floor_number))   
                % ak je farba rovna farbe poschodia alebo na farbe nezalezi   
                ur.RG2(0,50,0,false,false);     % uchopenie kocky so silou 50 N
                while ur.gripper_pose > 15      % osetrenie chyby ak sa gripper nezatvori
                    ur.RG2(0,50,0,false,false);
                    if k > 5 ;return; end
                    k = k + 1;
                end
                k = 1; m = 1;
                while ur.gripper_pose < 8           % Kontrola ci gripper uchopil kocku alebo nie
                    ur.RG2(50,30,0,false,false);
                    TextArea.Value = ('Block is missing!');
                    ur.movel(position_1);
                    pause(4);
                    ur.movel(start_position);
                    ur.RG2(0,50,0,false,false);
                    while ur.gripper_pose > 15      % osetrenie chyby ak sa gripper nezatvori
                    ur.RG2(0,50,0,false,false);
                    if m > 5 ;return; end
                    m = m + 1;
                    end
                    if k > 5 ;return; end
                    k = k + 1;
                end
                k = 1;

                ur.movel(position_1);
                ur.movej(position_above_measure_1);
                ur.movel(measure_position_1_1);
                ur.RG2(50,40,0,false,false);        % roztvorenie gripperu na sirku 50 mm so silou 40 N
                while ur.gripper_pose < 40
                    ur.RG2(50,40,0,false,false);
                    if k > 5 ;return; end
                    k = k + 1;
                end
                k = 1;

                ur.movel(measure_position_2)        % otocenie gripperu k premeraniu kocky

                ur.RG2(0,1,0,false,false);
                while ur.gripper_pose > 40
                    ur.RG2(0,2,0,false,false);
                    if k > 5 ;return; end
                    k = k + 1;
                end
                k = 1;
                %% Meranie kocky a prepocet novych premennych
                length_of_block = ur.gripper_pose;     % nacitanie sirky roztvorenia gripperu
                previous_block = index_of_block;       % pamat predoslej kocky
                previous_length_of_object = length_of_object;   % pamat predoslej dlzky 1 poschodia objektu          
                if length_of_block > 20            % dlzka objektu viac ako 20 mm
                    length_of_object = length_of_object - 2;    % odcitanie 2 "blokov" z dlzky
                    index_of_block = 2;                         % kocka je 2x4 piny(velka)
                    step = index_of_block;                      % krok je velkost kocky
                else                                            % mala kocka
                    length_of_object = length_of_object - 1;
                    index_of_block = 1;
                    step = index_of_block;
                    position_above_end(1) = position_above_end(1) + length_of_small_block/4;...    
                    % 1. kocka malá -> posun pozicie o stvrt kocky
                end

                if max(previous_length_of_object == corner_index_big_grip) && index_of_block == 2 % robot je v rohu pre pozdlzny uchop a velkost kocky je 2
                    big_grip = 1;                   % pozdlzne uchopenie kocky
                    if move == 1                         % prepinanie medzi pohybmi a smermi korekcie
                        move = 2;
                        correction = 4;
                    elseif move == 2
                        move = 3;
                        correction = 2;
                    elseif move == 3
                        move = 4;
                        correction = 3;
                    end
                end
                %% Posudenie vhodnosti kocky
                if previous_length_of_object == 2 && index_of_block == 1      % ak treba kocku 2x2 a zoberie 2x4 tak ju zahodi
                        length_of_object = length_of_object + 1;              % vratenie odcitanej velkosti bloku
                        trash_throw(ur);                                      % zahodenie kocky 3 
                        TextArea.Value = 'I need 2x4 block';
                elseif  previous_length_of_object == 3 && index_of_block == 2 % ak su do konca 3 bloky a zoberie 2x4 kocku
                        length_of_object = length_of_object + 2;         
                        trash_throw(ur);                
                        TextArea.Value = 'Cant place this block to finish building';
                else 
                    if iteration >= 1                                       % ak je po prvej iteracii
                        if previous_block == 1 && index_of_block == 2       % ak posledny blok bol 2x2 a dalsi je 2x4
                            step = 1.5;
                        elseif previous_block == 2 && index_of_block == 1   % ak posledny blok bol 2x4 a dalsi je 2x2
                            step = 1.52;
                        end
                        position_above_end = last_position;                 % nastavenie poslednej pozicie bloku ako novu poziciu
%% Nastavenie konecnej na zaklade smeru pohybu robota
                        switch move     % switch pre pohyby
                            % Pohyb dolava
                            case 1      
                                position_above_end(1) = position_above_end(1) - step * length_of_small_block; % posun v - x o nastaveny krok
                                if length_of_object == corner_index_normal(1)   % prechod na pohyb dopredu
                                    move = 2;
                                    correction = 4;     % nastavenie korekcie do + y
                                end
                            % Pohyb dopredu
                            case 2                            
                                if big_grip == 1 || rotation_after_big_grip == 1
                                    if big_grip == 1    % ak je nastaveny pozdlzny uchyt
                                        if previous_block == 2  % predchadzajúci blok bol 2x4
                                            step = 1.5;
                                            position_above_end(1) = position_above_end(1) -  step * length_of_small_block;
                                            step = 0.45;
                                        else            % predchadzajúci blok bol 2x2
                                            step = 1;
                                            position_above_end(1) = position_above_end(1) -  step * length_of_small_block;
                                            step = 0.5;
                                        end
                                    elseif rotation_after_big_grip == 1     % rotacia po pozdlznom uchyte ja aktivovana
                                        step = 0.07;
                                        position_above_end(1) = position_above_end(1) + step * length_of_small_block; % posun v osi - x      
                                        if index_of_block == 2 % aktualny blok je 2x4
                                          step = 2;
                                        else 
                                            step = 1.5; 
                                        end
                                    end
                                elseif  big_grip == 0 && previous_length_of_object == corner_index_normal(1)   % ak je aktivovany priecny uchyt spolu so situaciou na rohu
                                    if previous_block == 2   % ak posledny blok bol 2x4
                                            step = 0.4;
                                            position_above_end(1) = position_above_end(1) - step * length_of_small_block;
                                        if index_of_block == 2  % aktualny blok je 2x4
                                            step = 1.43;
                                        else                    % aktualny blok je 2x2
                                            step = 0.95;
                                        end
                                    elseif previous_block == 1   % ak posledny blok bol 2x2
                                            step = 0.07;
                                            position_above_end(1) = position_above_end(1) + step * length_of_small_block;
                                       if index_of_block == 2    % aktualny blok je 2x4
                                            step = 1.41;
                                       else                     % aktualny blok je 2x2
                                            step = 0.95;
                                        end
                                    end
                                end
        
                                if length_of_object == corner_index_normal(2) % prechod na posuv doprava
                                    move = 3;
                                    previous_move = 2;
                                    correction = 2;     % nastavenie korekcie do + y
                                end
                                position_above_end(2) = position_above_end(2) +  step * length_of_small_block;
                            % Pohyb doprava
                            case 3                           
                                if big_grip == 1 || rotation_after_big_grip == 1
                                    if big_grip == 1    % ak je nastaveny pozdlzny uchyt
                                        if previous_block == 2  % predchadzajuci blok bol 2x4
                                            step = 1.5;
                                            position_above_end(2) = position_above_end(2) +  step * length_of_small_block; % posuv do + y
                                            step = 0.5;
                                        else            % predchadzajuci blok bol 2x2
                                            step = 1;
                                            position_above_end(2) = position_above_end(2) +  step * length_of_small_block; % posuv do + y
                                            step = 0.5;
                                        end
                                    elseif rotation_after_big_grip == 1     % rotacia po pozdlznom uchyte ja aktivovana
                                        step = 0.02;
                                        position_above_end(2) = position_above_end(2) + step * length_of_small_block; % posun v osi + y      
                                        if index_of_block == 2 % aktualny blok je 2x4
                                          step = 1.95;
                                        else 
                                            step = 1.45; 
                                        end
                                    end
                                elseif  big_grip == 0 && previous_length_of_object == corner_index_normal(2)   % ak je aktivovany priecny uchyt spolu so situaciou na rohu
                                    if previous_block == 2       % ak posledny blok bol 2x4
                                            step = 0.53;
                                            position_above_end(2) = position_above_end(2) + step * length_of_small_block;
                                        if index_of_block == 2   % aktualny blok je 2x4
                                            step = 1.47;
                                        else                     % aktualny blok je 2x2
                                            step = 0.98;
                                        end
                                    elseif previous_block == 1   % ak posledny blok bol 2x2
                                            step = 0.05;
                                            position_above_end(2) = position_above_end(2) + step * length_of_small_block;
                                       if index_of_block == 2    % aktualny blok je 2x4
                                            step = 1.47;
                                       else                      % aktualny blok je 2x2
                                            step = 0.95;
                                        end
                                    end
                                end
                                if length_of_object == corner_index_normal(3) % prechod na posuv dozadu         
                                    move = 4;
                                    previous_move = 3;
                                    correction = 3;     % nastavenie korekcie do - y
                                end                     
                                position_above_end(1) = position_above_end(1) +  step * length_of_small_block;
                            % Pohyb dozadu
                            case 4   
                                if big_grip == 1 || rotation_after_big_grip == 1
                                    if big_grip == 1            % ak je nastaveny pozdlzny uchyt
                                        if previous_block == 2  % predchadzajúci blok bol 2x4
                                            step = 1.51;
                                            position_above_end(1) = position_above_end(1) +  step * length_of_small_block; % posuv do + x
                                            step = 0.48;
                                        else                    % predchadzajúci blok bol 2x2
                                            step = 1.03;
                                            position_above_end(1) = position_above_end(1) +  step * length_of_small_block; % posuv do + x
                                            step = 0.46;
                                        end
                                    elseif rotation_after_big_grip == 1 && previous_length_of_object ~= 2   % pozdlzny uchyt a nieje to posledna kocka
                                        step = 0.045;
                                        position_above_end(1) = position_above_end(1) + step * length_of_small_block; % posun v osi + x      
                                        if index_of_block == 2 % aktualny blok je 2x4
                                          step = 1.92;
                                        else 
                                            step = 1.53; 
                                        end
                                    end
                                elseif  big_grip == 0 && previous_length_of_object == corner_index_normal(3)   % ak je aktivovany priecny uchyt spolu so situaciou na rohu
                                    if previous_block == 2   % ak posledny blok bol 2x4
                                            step = 0.55;
                                            position_above_end(1) = position_above_end(1) + step * length_of_small_block;
                                        if index_of_block == 2  % aktualny blok je 2x4
                                            step = 1.6;
                                        else                    % aktualny blok je 2x2
                                            step = 1.08;
                                        end
                                    elseif previous_block == 1   % ak posledny blok bol 2x2
                                            step = 0.01;
                                            position_above_end(1) = position_above_end(1) - step * length_of_small_block;
                                       if index_of_block == 2   % aktualny blok je 2x4
                                            step = 1.6;
                                       else                     % aktualny blok je 2x2
                                            step = 1.08;
                                        end
                                    end
                                end
                                % zasunutie poslednej kocky
                                if previous_length_of_object == 2 && index_of_block == 2   % prechadzajuca dlzka poschodia je 2 a akt. blok je 2x4                                  
                                        step = 0.02;
                                        position_above_end(1) = position_above_end(1) + step * length_of_small_block;
                                        if previous_block == 2  % posledna kocka bola 2x4
                                            step = 1.58;
                                        else                    % posledna kocka bola 2x2
                                            step = 1;
                                        end
                                   last_block = 1;
                                end
                           position_above_end(2) = position_above_end(2) -  step * length_of_small_block;
                        end                 
                    end            
                iteration = iteration + 1;  % indikacia prveho zasunutia

%%  Uchopovanie pozdlžne/priečne
                % Priecne uchopovanie
                if  big_grip == 0       
                    ur.RG2(50,40,0,false,false);
                    while ur.gripper_pose < 40
                        ur.RG2(50,40,0,false,false);
                        if k > 5 ;return; end
                         k = k + 1;
                     end
                     k = 1;
                    
                    if last_block == 1      % uchop poslednej kocky
                        ur.movej(measure_position_1_2);
                    else
                        ur.movej(measure_position_1);    % rotacia pre priecne uchopenie kocky
                    end
                   
                    ur.RG2(0,50,0,false,false); 
                    while ur.gripper_pose > 15
                        ur.RG2(0,50,0,false,false);
                        if k > 5 ;return; end
                        k = k + 1;
                    end
                    k = 1;
                    
                    ur.movel(position_above_measure_1);
                    % normalny prechod rohu alebo zasunutie kocky po pozdlznom uchyte
                    if ((move == 2 || move == 3 || move == 4) && max(previous_length_of_object == corner_index_normal))...
                       || rotation_after_big_grip == 1 
                       % pri pohybe doprava
                       if rotation_after_big_grip == 1 || max(previous_length_of_object == corner_index_normal)  
                           if (move == 2 || move == 4) && previous_move ~= 3
                                ur.movej(position_above_measure_2);
                           % pri pohybe dopredu
                           elseif previous_move == 2
                                ur.movej(position_above_measure_2);
                           end
                       end
              % nastavenie rotacie gripperu poslednej pozicie ako aktualna
                        position_above_end(4) = ur.pose(4);
                        position_above_end(5) = ur.pose(5);
                        position_above_end(6) = ur.pose(6);
                % zaistenie kladnych rotacnych suradnic konecnej polohy
                            if position_above_end(5) < 0
                                position_above_end(5) = position_above_end(5) * -1; end
                            if  position_above_end(4) < 0
                                position_above_end(4) = position_above_end(4) * - 1; end
                            if  position_above_end(6) < 0
                                position_above_end(6) = position_above_end(6) * - 1; end                        
                    end
                    rotation_after_big_grip = 0;
                % Pozdlzny uchop
                else            
                    ur.movel(position_above_measure_2);
                    if move == 2 || move == 4 % rotacia pri prechode na pohyb dopredu a dozadu
                        ur.movej(position_above_measure_1);
                        position_above_end(4) = ur.pose(4);
                        position_above_end(5) = ur.pose(5);
                        position_above_end(6) = ur.pose(6);
                    end
                    rotation_after_big_grip = 1;
                end               
                ur.movel(position_above_end);       % Pohyb nad koncovu poziciu
                %% Zasúvanie kocky s korekciou
                forceAxis01 = [0 0 1 0 0 0];        % ktora osa bude riadena silovo(1) a polohovo(0)
                forces01 = [0 0 -32 0 0 0];         % hodnota sil pre silove riadenie
                lim01 = [0.003 0.003 0.01 0.1 0.1 0.1]; % rychlosti zasuvania pre silove a tolerancia pre polohove riadenie
                r = 0;                              % polomer korekcie
                correction_i = 0;                   % iteracia korekcie
                
                while ur.pose(3) > correction_depth(floor_number)   % dokym nebude kocka zasunuta
                    ur.apply_force(forceAxis01,forces01,lim01)      % aplikovanie sily
                        while ur.force(3) < 28      % posobi silou pokial nieje presiahnuta hranica 27 N
                            pause(0.002)
                        end
                        ur.stop;        % zastavenie robota  
                      if ur.pose(3) < correction_depth(floor_number)   % ak je poloha kocky mensia ako korekcna hlbka
                            break
                      else
                            base_pose = position_above_end;
                            actual_pose = base_pose;
                            ur.movel(base_pose);
                            correction_i = correction_i + 1;    % inkrementacia iteracie korekcie
                            if correction_i == 4
                            r = r + 1;          % inkrementacia polomeru korekcie
                            correction_i = 0;
                            end
                         switch correction      % zmena smeru korekcie
                            case 1              % smer - osi x
                                actual_pose(1) = actual_pose(1) - r * 0.00015; % posun korekcnej pozicie
                                ur.movej(actual_pose);      % pohyb do novej pozicie
                                correction = 2;
                            case 2              % smer + osi x
                                actual_pose(1) = actual_pose(1) + r * 0.00015;
                                ur.movej(actual_pose);
                                correction = 3;
                            case 3              % smer - osi y                  
                                actual_pose(2) = actual_pose(2) - r * 0.00015;
                                ur.movej(actual_pose);
                                correction = 4;
                            case 4              % smer + osi x
                                actual_pose(2) = actual_pose(2) + r * 0.00015;
                                ur.movej(actual_pose);
                                correction = 1;
                         end
                      end
                end
                
                last_position = position_above_end;     % pamat poslednej pozicie kocky
                
                ur.RG2(50,50,0,false,false);            % roztvorenie gripperu na sirku 50 mm silou 50 N
                while ur.gripper_pose < 40
                    ur.RG2(50,50,0,false,false);
                    if k > 5 ;return; end
                    k = k + 1;
                end
                k = 1;
                
                ur.movel(position_above_end);           % pohyb nad poziciu kde bola zasunuta kocka
              
                whole_object_length = (length(floor)-floor_number) * defined_length_of_object + length_of_object;   % prepocet dlzky celkoveho objektu
                percentage_of_build = round((1 - whole_object_length/(defined_length_of_object*max(floor)))*100,0);    % prepocet % faze stavby
                Gauge.Value = percentage_of_build;         % vykreslenie v GUI
               end
            elseif max(color_number == defined_color) == 1  % ak je na rade nespravna farba na akt. poschodie ale na dalsie je vhodna
                throw_back(ur,TextArea);    % vlozenie kocky naspat na koniec zasobnika
            else
                trash_throw(ur,TextArea);   % zahodenie kocky
            end
        else
            TextArea.Value = 'The block is missing!Please insert a lego block.';
            pause(4);
        end
        big_grip = 0;           % nastavenie na priecne uchopovanie
        previous_move = 0;      % vymazanie pamati posledneho pohybu
        if length_of_object == 0
            last_block = 0;         % zrusenie indikacie posledneho bloku
        end
    end
end
TextArea.Value = 'Complete!';
ur.movej(default_position);
end