%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                           %
%        Fanuc movement simulator           %
%                                           %
%    Gon�alo Alberto Ramos Carpinteiro      %
%                N� 49642                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%
%   Clear Memory  %
%%%%%%%%%%%%%%%%%%%

% Clear all memory and close all figures.
close all; clc

try
    % Disconnect and clean up the server connection TCP/IP. 
    fclose(t); 
    delete(t); 
    clear t 
catch
end

try
    % Disconnect and clean up the serial Port. 
    fclose(s); 
    delete(s); 
    clear s 
catch
end

% Disconnect and clean up the server connection TCP/IP. 
%fclose(t); 
%delete(t); 
% Disconnect and clean up the serial Port. 
%fclose(s); 
%delete(s); 

% %%%%%%%%%%%%%%%%%
% % Video Creator %
% %%%%%%%%%%%%%%%%%
%
Fig = figure('position',[100 50 600 600],'color',[1 1 1]); 
% aviobj= VideoWriter('simulator.avi');
% aviobj.FrameRate=100;
% aviobj.Quality = 100;
% a = axes('Parent',Fig);
% axis(a,'tight');
% set(a,'nextplot','replacechildren');
% opengl('software');
% open(aviobj);

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       Open TCP/IP        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Create TCP/IP object 't'. Specify server machine and port number. 
t = tcpip('192.168.0.230', 4900); 

% Set size of receiving buffer, if needed. 
set(t, 'InputBufferSize', 30000); 

% Open connection to the server. 
fopen(t);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      Open Serial Port    %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Create serial object 's'. Specify port and baudrate number. 
s = serial('/dev/ttyUSB2', 'BaudRate', 9600);

% Open connection to the port. 
fopen(s);

%%%%%%%%%%%%%%%%%%
% Init Fanuc TPP %
%%%%%%%%%%%%%%%%%%

% Transmit data to the server. 
fprintf(t, 'RUNTPP\nCARPSIMU\n1\n');
% Data for confirmation.
[msg, bytesread]=fgetl(t);

count=0;

%%
while(count<2000)

%%%%%%%%%%%%%%%%%%%%%%%%%
% Get Data From Arduino %
%%%%%%%%%%%%%%%%%%%%%%%%%

% Get data from serial port to variable read_s.
flushinput(s);
flushoutput(s);
readed=0;

data_ok=0;

while(data_ok==0)
    
    readed=0;

    while(readed==0)
        if s.BytesAvailable > 0
            read_s=s.fgetl;
            readed=1;
        end
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Processing the data from Arduino %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    data_a0 = 0; data_a1 =0; data_a2 =0; data_a3=0;

    % Isolate the data.
    if(strfind(read_s, 'A0:')~=0)

        % Sensor Number zero.
        A0=read_s(strfind(read_s, 'A0:')+3:(strfind(read_s,'A1:')-1));
        A0=str2double(A0);
        data_a0=1;
    end

    if(strfind(read_s, 'A1:')~=0)
        % Sensor Number one.
        A1=read_s(strfind(read_s, 'A1:')+3:(strfind(read_s,'A2:')-1));
        A1=str2double(A1);
        data_a1=1;
    end

    if(strfind(read_s, 'A2:')~=0)
        % Sensor Number two.
        A2=read_s(strfind(read_s, 'A2:')+3:(strfind(read_s,'A3:')-1));
        A2=str2double(A2);
        data_a2=1;
    end

    if(strfind(read_s, 'A3:')~=0)
        % Sensor Number three.
        A3=read_s(strfind(read_s, 'A3:')+3:(strfind(read_s,'End')-1));
        A3=str2double(A3);
        data_a3=1;
    end

    % Check if all data was recived.
    data_ok=data_a0 && data_a1 && data_a2 && data_a3;
end

% Convert the analog signal to voltage.
if data_ok == 1
    % Sensor 0.
    A0=A0*5/1023;
    % Sensor 1.
    A1=A1*5/1023;
    % Sensor 2.
    A2=A2*5/1023;
    % Sensor 3.
    A3=A3*5/1023;

    % Calibrate with preview calibration curves.
    % Sensor 0.
    a0=241.5631*exp(-3.8846*A0)+51.0816*exp(-0.7284*A0);
    % Sensor 1.
    a1=172.9317*exp(-3.7906*A1)+43.3609*exp(-0.6018*A1);
    % Sensor 2.
    a2=159.7164*exp(-3.4773*A2)+44.8366*exp(-0.6893*A2);
    % Sensor 3.
    a3=179.4828*exp(-3.5646*A3)+41.8678*exp(-0.5470*A3);
    
else
    disp('Merd...')
end

%%%%%%%%%%%%%%%%%%%%%%%%%
% Send Request to Fanuc %
%%%%%%%%%%%%%%%%%%%%%%%%%

% Send request to fanuc, GETCRCPOS!
fprintf(t, 'GETCRCPOS');

%%%%%%%%%%%%%%%%%%%%%%%
% Get Data From Fanuc %
%%%%%%%%%%%%%%%%%%%%%%%

% Get data from server to variable CP.
% read text Current position.
[msg, bytesread]=fgetl(t);
while(~strcmp(msg,'Current position'))
    [msg, bytesread]=fgetl(t);
end
% read xyz and convert to numbers.
[msg, bytesread]=fgetl(t);
[XYZ]=str2num(msg);
% read wpr and convert to numbers.
[msg, bytesread]=fgetl(t);
[WPR]=str2num(msg);
% read redundancy and convert to numbers.
[msg, bytesread]=fgetl(t);
% data for confirmation.
[msg, bytesread]=fgetl(t);
% 
% if (msg(1)=='F'); RED(1)=1; else RED(1)=0; end
% if (msg(3)=='U'); RED(2)=1; else RED(2)=0; end
% if (msg(5)=='T'); RED(3)=1; else RED(3)=0; end
% [RED(4:6)]=str2double(msg(6:size(msg,2)));

% buid CRCPOS variable.
Cpos=[XYZ WPR];


%%%%%%%%
% Axis %
%%%%%%%%

% Axis -> P.
run seixos; 


%%%%%%%%%%%%%%%%%%%%%%%%
% Draw Plan 2nd Method %
%%%%%%%%%%%%%%%%%%%%%%%%

% Draw the grid.
grid on

%%%%%%%%%%%%%%%
% Origin Axis %
%%%%%%%%%%%%%%%

% fill3(P(1,:),P(2,:),P(3,:),'y');
% hold on

%%%%%%%%%%%
% Z value %
%%%%%%%%%%%

Z=[a0 a1 a2 a3]*10;

%%%%%%%%%%%%%%%%%
% X and Y value %
%%%%%%%%%%%%%%%%%

X=[0 -35 35 0];
Y=[-35 0 0 35];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Combinations for method 2 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

cc = combnk(1:4,3);                                 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Some variables to store data %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Pa=[];
Pb=[];
Pc=[];
Pd=[];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%           Method 2           %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for n=1:size(cc,1)

    pos=cc(n,:);
    x1= X(pos(1)); y1= Y(pos(1));z1= Z(pos(1)); 
    x2= X(pos(2)); y2= Y(pos(2));z2= Z(pos(2));
    x3= X(pos(3)); y3= Y(pos(3));z3= Z(pos(3));
     
    xf=[x1 x2 x3];
    yf=[y1 y2 y3];
    zf=[z1 z2 z3];

    %%%%%%%%
    % Plan %
    %%%%%%%%

    A=  (y1*z2 - y2*z1 - y1*z3 + y3*z1 + y2*z3 - y3*z2)/(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);
    B= -(x1*z2 - x2*z1 - x1*z3 + x3*z1 + x2*z3 - x3*z2)/(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);
    C=  (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2)/(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);

    %%%%%%%%%%%%%%%%
    % D calculator %
    %%%%%%%%%%%%%%%%
     
    % a/d x + b/d y + c/d z == 1
    % A x + B y + C z == 1
    Zpt=A/C*x3+B/C*y3;
    D=z3+Zpt;

    a=A*D; b=B*D; c=C*D; d=D;

    % A B C and D.
    Pa=[Pa a]; Pb=[Pb b]; Pc=[Pc c]; Pd=[Pd d];
end

% Mean of A, B, C and D of each plan.    
aa=mean(Pa); bb=mean(Pb); cc=mean(Pc); dd=mean(Pd);

% X value to meshgrid.
x=-45:15:45;
[Xp,Yp] = meshgrid(x);

% Equation of Z based on plan equation.
Zp=-aa/cc*Xp-bb/cc*Yp+dd/cc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Transformtions 2->x  3->y  4->z ;  5->x  6->y  7->z %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Cpos(4) = degtorad(Cpos(4));
Cpos(5) = degtorad(Cpos(5));
Cpos(6) = degtorad(Cpos(6));

T0=Trans(Cpos(1),Cpos(2),Cpos(3));
TR = RPY([Cpos(4) Cpos(5) Cpos(6)]);
% T1=Rot('x',Cpos(4));
% T2=Rot('y',Cpos(5));
% T3=Rot('z',Cpos(6));

% Final Transformation
% Tf=T0*T1*T2*T3;
Tf=T0*TR;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Draw the axis of end efector %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


P1=Tf*P;
fill3(P1(1,:),P1(2,:),P1(3,:),'b');

hold on

%%%%%%%%%%%%%%%%%%%%%%%
% Points from sensors %
%%%%%%%%%%%%%%%%%%%%%%%

Pnts=[X;Y;Z;ones(1,4)];
Pts=Tf*Pnts;
plot3(Pts(1,:),Pts(2,:),Pts(3,:),'r*');

%%%%%%%%%%%%%%%%%%%
% Sensors Origins %
%%%%%%%%%%%%%%%%%%%

Zo=[40 70 70 40];
Org=[X;Y;Zo;ones(1,4)];
Orgf=Tf*Org;
plot3(Orgf(1,:),Orgf(2,:),Orgf(3,:),'*g');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Line to connect the origin points to sensor points %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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)])

%%%%%%%%%%%%%%%
% Ground Plan %
%%%%%%%%%%%%%%%

xxx=mean(Pts(1,:));
yyy=mean(Pts(2,:));
zzz=mean(Pts(3,:));

Xgg=[xxx+100 xxx-100 xxx-100 xxx+100];
Ygg=[yyy+100 yyy+100 yyy-100 yyy-100];
Zgg=[zzz-50 zzz-50 zzz-50 zzz-50];

%fill3(Xgg,Ygg,Zgg,'g');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Draw the plan from the method %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Plane=[Xp(:)';Yp(:)';Zp(:)';ones(1,size(Zp,2)*size(Zp,1))];

Planef=Tf*Plane;

    plot3(Planef(1,:),Planef(2,:),Planef(3,:),'-k')

%%%%%%%%%%%%%%%%%%%%
% Some graph tools %
%%%%%%%%%%%%%%%%%%%%

title('Simulation')
xlabel('x')
ylabel('y')
zlabel('z')
grid on
%     view(-50,20)
%     axis equal
width=500;
cx = -80;
cy = -880;
cz = -400;
axis([ cx-width/2 cx+width/2 cy-width/2 cy+width/2 cz-width/2 cz+width/2])
%     axis square
%    11 view(-90,0)

%%%%%%%%%%%%%%%%%%%
% Get video frame %
%%%%%%%%%%%%%%%%%%%

%             F = getframe(Fig);
%             writeVideo(aviobj,F);    
pause(0.01)

hold off

count=count+1;

end

%%
%%%%%%%%%%%%%
% Close all %
%%%%%%%%%%%%%

try
    
% Disconnect and clean up the server connection TCP/IP. 
fclose(t); 
delete(t); 
clear t 
catch
end

try
% Disconnect and clean up the serial Port. 
fclose(s); 
delete(s); 
clear s 
catch
end
% % Close video
%         for k=1:400
%             F = getframe(Fig);
%             writeVideo(aviobj,F);
%         end
%         close(aviobj);












