clc
clear all
delete(instrfind({'Port'},{'COM3'}))

s= serial('COM3');
set(s,'BaudRate',9600);
fopen(s);


Initialpoint=[15;0;0];
InitialpointB1=[-7;0;0];
InitialpointB2=[7;0;0];
OP=[0,0,0];

%h = line([0;x],[0;y],[0;z]);
%hold off

%set(h,'linewidth',5);

% Infinite loop
for i=1:550

    % Reading the serial port
    str = fscanf(s);

    % Finding the letter a for seperation (for first sensor)
    indexX1 = find(str == 'X');
    % First data obtained
    strX1 = str(1:indexX1-1);

    indexY1 = find(str == 'Y');
    % Second data is obtained
    strY1 = str(indexX1+1:indexY1-1);

    indexZ1 = find(str == 'Z');

    % third data obtained
    strZ1 = str(indexY1+1:indexZ1-1);

    % Finding the letter a for seperation(for second sensor)
    indexX2 = find(str == 'A');
    % First data obtained
    strX2 = str(indexZ1+1:indexX2-1);

    indexY2 = find(str == 'B');
    % Second data is obtained
    strY2 = str(indexX2+1:indexY2-1);

    indexZ2 = find(str == 'C');

    % third data obtaineed
    strZ2 = str(indexY2+1:indexZ2-1);


    % Converting string to double precision value (Gyro)
    eulerX1 = str2double(strX1);
    % Converting string to double precision value (Acc)
    eulerY1 = str2double(strY1);
    eulerZ1 = str2double(strZ1);

    eulerX2 = str2double(strX2);
    % Converting string to double precision value (Acc)
    eulerY2 = str2double(strY2);
    eulerZ2 = str2double(strZ2);

    x=eulerX1;
    y=eulerY1;
    z=eulerZ1;

    e=eulerX2;
    f=eulerY2;
    g=eulerZ2;

    AngP2=[x y z];



    %Rotation angles ZYX seguidores(deg)
    angles=[z -y x];
    angles2=[g -f e];
    %Rotation angles XYZ(rad)
    angles=angles*pi()/180;
    angles2=angles2*pi()/180;

    R=zeros(3,3);
    R2=zeros(3,3);

    %Rotación en Z
    alpha=angles(1,1);
    %Rotación en Y
    beta=angles(1,2);
    %Rotación en X
    gamma=angles(1,3);

    %Second sensor Rotación en Z
    alpha2=angles2(1,1);
    %Rotación en Y
    beta2=angles2(1,2);
    %Rotación en X
    gamma2=angles2(1,3);

    %Rotation matrices
    R(1,1)= cos(alpha)*cos(beta);
    R(1,2)= cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma);
    R(1,3)=cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma);
    R(2,1)=sin(alpha)*cos(beta);
    R(2,2)=sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma);
    R(2,3)=sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma);
    R(3,1)=-sin(beta);
    R(3,2)=cos(beta)*sin(gamma);
    R(3,3)=cos(beta)*cos(gamma);

    R2(1,1)= cos(alpha2)*cos(beta2);
    R2(1,2)= cos(alpha2)*sin(beta2)*sin(gamma2)-sin(alpha2)*cos(gamma2);
    R2(1,3)=cos(alpha2)*sin(beta2)*cos(gamma2)+sin(alpha2)*sin(gamma2);
    R2(2,1)=sin(alpha2)*cos(beta2);
    R2(2,2)=sin(alpha2)*sin(beta2)*sin(gamma2)+cos(alpha2)*cos(gamma2);
    R2(2,3)=sin(alpha2)*sin(beta2)*cos(gamma2)-cos(alpha2)*sin(gamma2);
    R2(3,1)=-sin(beta2);
    R2(3,2)=cos(beta2)*sin(gamma2);
    R2(3,3)=cos(beta2)*cos(gamma2);

    Finalpoint=R*Initialpoint;
    FinalpointB1=R2*InitialpointB1;
    FinalpointB2=R2*InitialpointB2;

    FinalX=Finalpoint(1,1);
    FinalY=Finalpoint(2,1);
    FinalZ=Finalpoint(3,1);
    FinalXYZ=[FinalX,FinalY,FinalZ];

    dx=FinalX-FinalpointB1(1,1);
    dy=FinalY-FinalpointB1(2,1);
    dz=FinalZ-FinalpointB1(3,1);

    FinalX2=FinalpointB2(1,1);
    FinalY2=FinalpointB2(2,1);
    FinalZ2=FinalpointB2(3,1);
    Final2XYZ=[FinalX2+dx,FinalY2+dy,FinalZ2+dz];

    %create figures
    [a,b,c]=cylinder2P(0.5,20,OP,FinalXYZ);
    hold on;
    [d,e,f]=cylinder2P(0.5,20,FinalXYZ,Final2XYZ);
    grid on;
    axis manual;
    hold off;
    axis([-30 30 -30 30 -30 30]);
    view(3);
    drawnow;

    Final2XYZ=Final2XYZ-FinalXYZ;

    %angle between sensors check
    Valor=dot(Final2XYZ,FinalXYZ);
    absoluto1=norm(FinalXYZ);
    absoluto2=norm(Final2XYZ);

    Angulo=180-acos(Valor/(absoluto1*absoluto2))*180/pi()

    pause(0.02);


end
delete(instrfind({'Port'},{'COM3'}));