% clear all
close all
clc

%% Vários
%Dados
l1=3;

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

%Bias para video
% gyro2=[0.9,1.6]
% gyro3=[0.9,1.1]

%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.95;

%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
ws0=[-gyro2(:,1) gyro2(:,2) -gyro3(:,1)]*pi/180;

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

%Matriz inicial para os acelerometros
p0_T_p1_a=p0_T_p1;


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

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

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

pause

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

%Contador
cont=0;

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

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

%Guardar dados
log_pos_s0=[0 0 0];
ang_euler.gyro.fi=0;
ang_euler.gyro.teta=0;
ang_euler.gyro.psi=0;

%Desvio
desv=RPY(-0.84*pi/180,4.5*pi/180,-6.7*pi/180);


%% 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));
        
        %Soma da posição da junta
        log_pos_s0=[log_pos_s0
                    log_pos_s0(end,:)+junta1'];
        
        
        %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));
        
        %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;
        
        %Angulos de euler pelos dados do gyro
        fi=atan2(p0_T_p1(2,1,end),p0_T_p1(1,1,end));
        ang_euler.gyro.fi(end+1)=fi;

        teta=atan2(-p0_T_p1(3,1,end),p0_T_p1(1,1,end)*cos(fi)+p0_T_p1(2,1,end)*sin(fi));
        ang_euler.gyro.teta(end+1)=teta;

        psi=atan2(p0_T_p1(3,2,end),p0_T_p1(3,3,end));
        ang_euler.gyro.psi(end+1)=psi;
        
%     end
%     
%     if matriz_trans==1
        %% Actualização da animação
        
        X=[0 p0_T_p1(1,4)];
        Y=[0 p0_T_p1(2,4)];
        Z=[0 p0_T_p1(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,:));
        
        %%%Video%%%%%%%%%%%%%%%%
%         F = getframe();
%         aviobj = addframe(aviobj,F);
        %%%%%%%%%%%%%%%%%%%%%%%%
        
%     end

end

% aviobj = close(aviobj);

%% Fanuc

%%Ex1
% Qi_0=[0 0 0 0 0 0]';
% Qf_0=[0 0 0 0 0 0]';
% N_0=72;
% Qi_1=[0 0 0 0 0 0]';
% Qf_1=[0 0 pi/2 pi/2 0 0]';
% N_1=182-72;
% Qf_2=[0 0 0 0 0 0]';
% Qi_2=[0 0 pi/2 pi/2 0 0]';
% N_2=301-182;
% Qf_3=[0 0 0 0 0 0]';
% Qi_3=[0 0 0 0 0 0]';
% N_3=719-301;

%Exp2
Qi_0=[0 0 0 0 0 0]';
Qf_0=[0 0 0 0 0 0]';
N_0=74;
Qi_1=[0 0 0 0 0 0]';
Qf_1=[pi/2 0 pi/2 pi/2 0 0]';
N_1=221-74;
Qf_2=[0 0 0 0 0 0]';
Qi_2=[pi/2 0 pi/2 pi/2 0 0]';
N_2=371-221;
Qf_3=[0 0 0 0 0 0]';
Qi_3=[0 0 0 0 0 0]';
N_3=906-371;

MQ_0=Linespacevect(Qi_0,Qf_0,N_0);
MQ_1=Linespacevect(Qi_1,Qf_1,N_1);
MQ_2=Linespacevect(Qi_2,Qf_2,N_2);
MQ_3=Linespacevect(Qi_3,Qf_3,N_3);

MQ=[MQ_0 MQ_1 MQ_2 MQ_3];

l2=1;l3=1;l4=1;l5=1;

DH=[pi/2    0       l2      pi/2
    pi/2    l3      0       0
    0       0       0       pi/2
    pi      0       l4      pi/2
    pi      0       0       pi/2
    0       0       l5      0];


%ex3
MDH=GenerateMultiDH(DH,MQ);

%ex5
AAA=CalculateRobotMotion(MDH);

ang_euler.fanuc.fi=[];
ang_euler.fanuc.teta=[];
ang_euler.fanuc.psi=[];

T=[0 0 1 0
   1 0 0 0
   0 1 0 0
   0 0 0 1];
for i=1:size(AAA,4)
    
    %Multiplicação de cada transformação para obter a transformação da
    %origem ao punho a cada instante
    A=eye(4);
    for n=(1:size(AAA,3))
        A=A*AAA(:,:,n,i);
    end
    A=A*T;
    
    %Calculo dos angulos de euler
    fi=atan2(A(2,1),A(1,1));
    ang_euler.fanuc.fi(end+1)=fi;

    teta=atan2(-A(3,1),A(1,1)*cos(fi)+A(2,1)*sin(fi));
    ang_euler.fanuc.teta(end+1)=teta;

    psi=atan2(A(3,2),A(3,3));
    ang_euler.fanuc.psi(end+1)=psi;

end


ang_euler.gyro.fi=ang_euler.gyro.fi*180/pi;
ang_euler.gyro.teta=ang_euler.gyro.teta*180/pi;
ang_euler.gyro.psi=ang_euler.gyro.psi*180/pi;
ang_euler.fanuc.fi=-ang_euler.fanuc.fi*180/pi;
ang_euler.fanuc.teta=-ang_euler.fanuc.teta*180/pi;
ang_euler.fanuc.psi=ang_euler.fanuc.psi*180/pi;



%% Integração dos eixos comuns

% sensor2=0;
% sensor3=0;
% 
% for i=2:size(gyro2,1)
% sensor2(i)=gyro2(i,2)*(log_tempo(i)-log_tempo(i-1))+sensor2(i-1);
% sensor3(i)=gyro3(i,2)*(log_tempo(i)-log_tempo(i-1))+sensor3(i-1);
% end

%% Velocidades angulares dos eixos em comum

% plot(log_tempo(1,1:end-1),gyro2(:,2),log_tempo(1,1:end-1),gyro3(:,2))