%% Just to check the fanuc angles are correct
close all; clc

%% Start communication with fanuc

try
    fclose(fanuc); 
    delete(fanuc);
    clear fanuc
catch
end

fanuc = tcpip('192.168.0.230', 4900); 
set(fanuc, 'InputBufferSize', 30000); 

fopen(fanuc);

%% Start communication with arduino

try
    fclose(arduino); 
    delete(arduino);
    clear arduino
catch
end

arduino = serial('/dev/ttyUSB4', 'BaudRate', 9600);
fopen(arduino);

%% Run a program

fprintf(fanuc, 'RUNTPP\nCARPSIMU\n1\n');
% Data for confirmation.
[msg, bytesread]=fgetl(fanuc);


%% Create a figure

Fig = figure('position',[100 50 600 600],'color',[1 1 1]);

%% Get position and plot

run seixos; 

CleanBuffer(fanuc)

width=1000;
cx = -80;
cy = -880;
cz = -400;

sensors(1,:) = [0 -350 350 0];
sensors(2,:) = [350 0 0 -350];
sensors(3,:) = [40 70 70 40];
sensors(4,:) = [1 1 1 1];


Zhor=[];

while(1)
    
    
    
    %Get fanuc position
    pos = getcrcpos(fanuc);
    
    transform = trans3(pos(1),pos(2),pos(3))*RPY([pos(6) pos(5) pos(4)]);
    
    %Get sensor values
    
    values = read_distances(arduino);
    distances = calibrate_distances(values)*10;
    
    %Draw sensors origins
    Org=sensors;
    Orgf=transform*Org;
    plot3(Orgf(1,:),Orgf(2,:),Orgf(3,:),'g*');
    
    hold on
    
    %Draw sensors position
    Pnts=sensors;
    Pnts(3,:)=distances;
    Pts=transform*Pnts;
    plot3(Pts(1,:),Pts(2,:),Pts(3,:),'r*');
    
    %Draw line
    line([Orgf(1,1) Pts(1,1)],[Orgf(2,1) Pts(2,1)],[Orgf(3,1) Pts(3,1)])
    line([Orgf(1,2) Pts(1,2)],[Orgf(2,2) Pts(2,2)],[Orgf(3,2) Pts(3,2)])
    line([Orgf(1,3) Pts(1,3)],[Orgf(2,3) Pts(2,3)],[Orgf(3,3) Pts(3,3)])
    line([Orgf(1,4) Pts(1,4)],[Orgf(2,4) Pts(2,4)],[Orgf(3,4) Pts(3,4)])
    
    %Get plan
    
    Pp=get_plan_meth_2(sensors(1,:),sensors(2,:),distances);
    a=Pp(1); b=Pp(2); c=Pp(3); d=Pp(4);
    Plan=get_plan(a,b,c,d);
    %Draw plan
    
    Planf=transform*Plan;
    plot3(Planf(1,:),Planf(2,:),Planf(3,:),'-k')
    
    
    object=transform*P;

    fill3(object(1,:),object(2,:),object(3,:),'b');

    axis([ cx-width/2 cx+width/2 cy-width/2 cy+width/2 cz-width/2 cz+width/2])
    grid on
    view(0,-1)
    xlabel('x')
    ylabel('y')
    zlabel('z')
    
     title(sprintf('Simulation  Z med:%3.2d',mean(Planf(3,:))))
    
    Zhor=[Zhor mean(Planf(3,:))];

    pause(0.1)
    hold off
end