% clear all
close all
clc

%Dados
l1=3;
l2=2;

%Definição da visualização
figure(1);
hold on;
grid on;
xlabel('X');ylabel('Y');zlabel('Z');
axis([-1 6 -1 6 -1 6]);
% view([90 0 0])
view(3)

% % Bias experiencias Maio
% gyro2=[1.22,2.12]
% gyro3=[1.204,1.1]

% % Bias experiencias Junho
% gyro2=[1.1,1.87]
% gyro3=[1.60,1.52]

%Aquisição de dados da base de dados
for i=4:size(database.id.mod2.gyro.data,1)-1
    gyro2(i,:)=(median(database.id.mod2.gyro.data(i-3:i,:))+[1.22,2.12]);
    gyro3(i,:)=(median(database.id.mod3.gyro.data(i-3:i,:))+[1.204,1.1]);
end

%Factor de escala
gyro2(:,1)=gyro2(:,1)*0.93;
gyro2(:,2)=gyro2(:,2)*1;
gyro3(:,1)=gyro3(:,1)*0.88;
gyro3(:,2)=gyro3(:,2)*0.94;

%Aquisição de dados do acelerometro e conversão para 
acel2(1:size(gyro2,1),:)=database.id.mod2.acel.data(1:size(gyro2,1),:)*9.81;
acel3(1:size(gyro2,1),:)=database.id.mod3.acel.data(1:size(gyro2,1),:)*9.81;

%Aquisição de tempos
log_tempo=(database.id.mod2.timestamp-database.id.mod2.timestamp(1))*tempo_estoiro_tmr1; %Zerar

%Colocar os eixos iguais
acel2(:,1:2)=-acel2(:,1:2);
acel3(:,1:2)=-acel3(:,1:2);

%Conversão para radianos
ws1=[-gyro2(:,1) gyro2(:,2) zeros(size(gyro2,1),1)]*pi/180; %Sensor 2
ws0=[-gyro3(:,1) gyro3(:,2) zeros(size(gyro2,1),1)]*pi/180; %Sensor 3

% %Acerto de eixos (T)
T2=[1.0000   -0.0193         0
    -0.0193    1.0000         0
         0         0    1.0000];
ws1=(T2*ws1')';

T3=[1.0000    -0.0051         0
   -0.0051    1.0000         0
         0         0    1.0000]; %Sensor 3
ws0=(T3*ws0')';

%Matriz inicial
p0_T_p1=[1 0 0 0
         0 1 0 l1
         0 0 1 0
         0 0 0 1];

p1_T_p2=[1 0 0 0
         0 1 0 l2
         0 0 1 0
         0 0 0 1];

%Matriz inicial para os acelerometros
p0_T_p1_a=p0_T_p1;

p1_T_p2_a=p1_T_p2;


%Dados Iniciais
X=[0 0 0];
Y=[0 l1 l1+l2];
Z=[0 0 0];

%Desenho do braço
h=line(X,Y,Z);
set(h,'LineWidth',2,'Marker','Diamond');

%Desenho dos eixos
AAA(:,:,1)=p0_T_p1;AAA(:,:,2)=p1_T_p2;
c=[1 2 3 4 5 6 7 8 9]; %Cores dos eixos
H=DrawFrames(AAA,seixos(0.6),c);

pause

%%%Video%%%%%%%%%%%%%%%%
% aviobj = avifile('exp1.avi');
% F = getframe();
%%%%%%%%%%%%%%%%%%%%%%%%

%Contador
cont=0;

%Psi inicial (como não há maneira de saber admite-se q é zero)
psi2=0;
psi3=0;

%Quando a matriz já estiver definida passa a 1
matriz_trans=0;

%Guardar dados
log_pos_s0=[0 0 0];
log_pos_s1=[0 0 0];

%% Algoritmo Principal dos sistemas diferenciais
for i=2:size(gyro2,1)
    
%     acel_ab2=sqrt(sum(acel2(i,:).^2));
%     acel_ab3=sqrt(sum(acel3(i,:).^2));
%     
%     %Parado
%     if acel_ab2>9.5 && acel_ab2<10 && acel_ab3>9.5 && acel_ab3<10 && min(ws0(i,:))>-0.15 && max(ws0(i,:))<0.15 && min(ws1(i,:))>-0.15 && max(ws1(i,:))<0.15
%         
%         cont=cont+1;% contador para saber quando está parado
%         
%         if cont>=15 %Só começa a fazer calculos depois de ter a certeza que está parado
%             %% Posição Absoluta -> Acelerómetros
%             
%             teta3=atan2(acel3(i,1),acel3(i,3)); %Eixo Y
%             fi3=atan2(acel3(i,2),sqrt(acel3(i,3)^2+acel3(i,1)^2)); %Eixo X 
%             
%             teta2=atan2(acel2(i,1),acel2(i,3))-teta3; %Eixo Y
%             fi2=atan2(acel2(i,2),sqrt(acel2(i,3)^2+acel2(i,1)^2))-fi3; %Eixo X
% 
%             p0_T_p1=RPY(psi3,teta3,fi3)*p0_T_p1_a;
%             p1_T_p2=RPY(psi2,teta2,fi2)*p1_T_p2_a;
%             p0_T_p2=p0_T_p1*p1_T_p2;
% 
%             matriz_trans=1;
%             
%         end
%         
%     elseif matriz_trans==1
        %% Movimento relativo -> Giroscópios
        
        cont=0; %Zerar assim q entra em movimento
        
        %Para gráficos do relatorio
        %Cálculo da rotação de cada junta
        junta1=ws0(i,:)'*(log_tempo(i)-log_tempo(i-1));
        junta2=((p1_T_p2(1:3,1:3)^-1)*(p1_T_p2(1:3,1:3)*ws1(i,:)'-ws0(i,:)'))*(log_tempo(i)-log_tempo(i-1));
        
        %Soma da posição da junta
        log_pos_s0=[log_pos_s0
                    log_pos_s0(end,:)+junta1'];
        log_pos_s1=[log_pos_s1
                    log_pos_s1(end,:)+junta2'];
        
        
        %Para efeitos de animação
        %Calculo do novo ponto
        pos_s0=(p0_T_p1(1:3,1:3)*ws0(i,:)')*(log_tempo(i)-log_tempo(i-1));
        pos_s1=(p1_T_p2(1:3,1:3)*ws1(i,:)'-ws0(i,:)')*(log_tempo(i)-log_tempo(i-1));
        
        %Calculo da matriz de transformação geometrica final
        p0_T_p1=RPY(pos_s0(3,1),pos_s0(2,1),pos_s0(1,1))*p0_T_p1;
        p1_T_p2=RPY(pos_s1(3,1),pos_s1(2,1),pos_s1(1,1))*p1_T_p2;
        p0_T_p2=p0_T_p1*p1_T_p2;
        
%     end
%     
%     if matriz_trans==1
        %% Actualização da animação
        
        X=[0 p0_T_p1(1,4) p0_T_p2(1,4)];
        Y=[0 p0_T_p1(2,4) p0_T_p2(2,4)];
        Z=[0 p0_T_p1(3,4) p0_T_p2(3,4)];

        pause(0.02);
        set(h,'XData',X,'YData',Y,'ZData',Z);

        %Actualização dos eixos
        p0_H_p1=p0_T_p1*seixos(0.6);
        set(H(2),'XData',p0_H_p1(1,:),'YData',p0_H_p1(2,:),'ZData',p0_H_p1(3,:));
        p0_H_p2=p0_T_p2*seixos(0.6);
        set(H(3),'XData',p0_H_p2(1,:),'YData',p0_H_p2(2,:),'ZData',p0_H_p2(3,:));

        %%%Video%%%%%%%%%%%%%%%%
%         F = getframe();
%         aviobj = addframe(aviobj,F);
        %%%%%%%%%%%%%%%%%%%%%%%%
        
%     end

end

% aviobj = close(aviobj);