%% Arduino/Matlab Komunikace
    clear all;
    close all;
    clc;
    delete(instrfindall);
%% Kontrola sériových objektů a připojení portu
    port = 'COM4';
    oldSerial = instrfind('Port', port); 
    if (~isempty(oldSerial)) 
        disp('POZOR:  Port je používán. Zkontroluj port. Zavírám program.')
        delete(oldSerial);
        a = true;
    else        
        disp('Připojuji serial port...');
    end    
%%  Nastavení seriove komunikace
    %Připojení Arduina přes COM port
    s = serial(port,'baudrate',9600,'terminator','CR','tag','Quad');
    %Počkej než přitečou data
    set(s, 'TimeOut', 5);  
    % Nastavení dostatečně velké paměti pro všechna data
    set(s, 'InputBufferSize',  390 );
    % Před zápisem dat je nutné port otevřít
    fopen(s);
    
    disp('Serial port otevřen');
    %% Připojení a nastavení V-REP  
    origine_table=[0,0.2,0.65];
    cube_dimensions=[0.06,0.06,0.06];
    width_offset=0.01;
    length_offset=0.03;
    end_test=0;
    vrep=remApi('remoteApi');
    vrep.simxFinish(-1);
    clientID=vrep.simxStart('127.0.0.1',19999,true,true,5000,5);
    disp('Vzdálený API server připojen');
    %Uložení potřebných objektů do proměnných
    [res,j1]=vrep.simxGetObjectHandle(clientID,'ROBOTIQ_85_active1',vrep.simx_opmode_blocking); 
    [res,j2]=vrep.simxGetObjectHandle(clientID,'ROBOTIQ_85_active2',vrep.simx_opmode_blocking); 
    [res,kuka_target]=vrep.simxGetObjectHandle(clientID,'target',vrep.simx_opmode_blocking); 
    [res,Proximity_sensor]=vrep.simxGetObjectHandle(clientID,'Proximity_sensor',vrep.simx_opmode_blocking); 
    
    %Proměnné z funkce Move L, pro pozdější vylepšení programu
    [r,p]=vrep.simxGetObjectPosition(clientID,kuka_target, -1, vrep.simx_opmode_blocking);
    
    %Proměnné z funkce GRIPPER, pro pozdější vylepšení programu 
    [r,p1]=vrep.simxGetJointPosition(clientID,j1,vrep.simx_opmode_blocking);
    [r,p2]=vrep.simxGetJointPosition(clientID,j2,vrep.simx_opmode_blocking);    
    
    disp('POZOR - Začni ovládat robota!');   
%% Komunikace, Vytvoření struktury seriove komunikace
    
    test = {};
    test.senzor = 1;
    test.time = 0;
    stara_pozice = [0, 0, 0];
   
    while 1
       % Načtení senzoru polohy
       [res,PSensor_distance, detectedPoint]= vrep.simxReadProximitySensor(clientID, Proximity_sensor, vrep.simx_opmode_blocking);
       test.PSensor=PSensor_distance;
       test.cube_size = detectedPoint(1,3)*1000;
       
       %Vytvoř strukturu JSON a odešli do portu COM  
       encoded_json = jsonencode(test);
       fprintf(s, '%s\n', encoded_json);      
       
       arduino=fscanf(s);
       
       try
               %Dekoduj strukturu JSON pro matlab
            b = jsondecode(arduino);
            b.time;
        
            fprintf(s, '%s\n', encoded_json);  
            b.pozice;
            act_pozice = reshape(b.pozice,1,3);
   
            moveL(clientID,kuka_target, act_pozice ,(b.speed))   
            gripper (clientID,b.open_close,j1,j2),
    
            stara_pozice = act_pozice;
            test.time = test.time;
                     
            arduino=fscanf(s);
       catch
            
       end   
    end       
%% Close connection    
    fclose(s);
    delete(s);
    clear s