%% Constantes

g_const_pos=0.05;
g_const_vel=0.1;
g_const_acel=0.1;
g_const_pos_ang=1;
g_const_media=0.0001;
% log_tempo=database.id.mod2.timestamp-database.id.mod2.timestamp(1); %Zerar
% tempo_estoiro_tmr1=0.049664; %conversão para seg
% log_tempo=log_tempo*tempo_estoiro_tmr1; %seg
% log_tempo_min=log_tempo/60; %min
modulo=3;
n_modulos=3;

%% Tratamento de dados

%�?s vezes não sao adquiridos o mesmo numero de dados para os 2 sensores, por isso elimina-se o ultimo dado de 1
if size(database.id.mod2.acel.data,1)>size(database.id.mod3.acel.data,1) && max(database.id.mod3.acel.data(1,:))~=0 && min(database.id.mod3.acel.data(1,:))~=0
    database.id.mod2.acel.data=database.id.mod2.acel.data(1:end-1,:);
    database.id.mod2.gyro.data=database.id.mod2.gyro.data(1:end-1,:);
    database.id.mod2.timestamp=database.id.mod2.timestamp(:,1:end-1);
elseif size(database.id.mod2.acel.data,1)<size(database.id.mod3.acel.data,1) && max(database.id.mod2.acel.data(1,:))~=0 && min(database.id.mod2.acel.data(1,:))~=0
    database.id.mod3.acel.data=database.id.mod3.acel.data(1:end-1,:);
    database.id.mod3.gyro.data=database.id.mod3.gyro.data(1:end-1,:);
    database.id.mod3.timestamp=database.id.mod3.timestamp(:,1:end-1);
end

%Passar para uma hipermatriz
for i=2:n_modulos
    A_g(:,:,i)=database.id.(['mod' num2str(i)]).acel.data;
    G(:,:,i)=database.id.(['mod' num2str(i)]).gyro.data;
    log_tempo(:,:,i)=database.id.(['mod' num2str(i)]).timestamp-database.id.(['mod' num2str(i)]).timestamp(1); %Zerar
end

% T=[1 0.9898-1
%    -(0.9898-1) 1];
% 
% G(:,:,modulo)=(T*G(:,:,modulo)')';

%Compensação Bias
% for i=1:size(G,1)
%     G(i,:,2)=G(i,:,2)+[1.1,2.12];
%     G(i,:,3)=G(i,:,3)+[0.8,1.072];
% end

% %Factor de Escala
% G(:,1,2)=G(:,1,2)*0.925;
% G(:,2,2)=G(:,2,2)*1;
% G(:,1,3)=G(:,1,3)*0.88;
% G(:,2,3)=G(:,2,3)*0.96;

log_tempo=log_tempo*tempo_estoiro_tmr1; %seg
log_tempo_min=log_tempo/60; %min

Acc=A_g*9.81; %m

% G(:,2,:)=-G(:,2,:); % gyroscope y axis needs to be inverted to match with accelerometer Y axis

%% Integração (aquisição de v,x e angulos de rotação)

% aceler_inicial=mean(Acc(1:end,:,modulo)); %Subtrai a média de tudo
% aceler_inicial=Acc(1,:,modulo); %Subtrai o primeiro
aceler_inicial=mean(Acc(1:20,:,modulo)); %Subtrai a média dos 20 primeiros
%Valores iniciais
log_acel=Acc(1,:,modulo)-aceler_inicial;%[0 0 0];
log_vel=[0 0 0];
log_pos=[0 0 0];
log_pos_ang=[0 0];
log_tempo_inst=0;


for i=1:(size(Acc)-1)
    
    %Aceleração, velocidade e posição
    acel_inst=Acc(i+1,:,modulo)-aceler_inicial; %Diferença da aceleração
%     acel_inst=(A2(i+1,:)+A2(i,:))/2;

    int_tempo=log_tempo(:,i+1,modulo)-log_tempo(:,i,modulo); %Intervalo de tempo
    vel_inst=acel_inst*int_tempo+log_vel(end,:); %Velocidade actual
    pos_inst=0.5*acel_inst*(int_tempo^2)+log_vel(end,:)*int_tempo+log_pos(end,:); %Posição actual
    
    %Posição angular
    %Compensação modulo 2: [1.2,0.57]
    %Compensação modulo 3: [0.88,0.28]
    pos_ang_inst=(G(i+1,:,modulo))*int_tempo+log_pos_ang(end,:);

    %Registo de valores
    log_acel=[log_acel
              acel_inst];
    log_vel=[log_vel
             vel_inst];
    log_pos=[log_pos
             pos_inst];
    log_pos_ang=[log_pos_ang
                 pos_ang_inst];
    log_tempo_inst=[log_tempo_inst
                    int_tempo];

end

% %Definição do gráfico 1 (aceleração com o efeito da gravidade)
% figure(1);
% plot(log_tempo(:,:,modulo),A_g(:,:,modulo));
% grid on;
% ylabel('g');xlabel('Tempo (s)');
% legend('X','Y','Z');
% axis([0 max(log_tempo(:,:,modulo)) -2 2])
% 
% %Definição do gráfico 2 (velocidade angular)
% figure(2);
% plot(log_tempo(:,:,modulo),G(:,:,modulo))
% axis([0 max(log_tempo(:,:,modulo)) min(min(G(:,:,modulo)))-5 max(max(G(:,:,modulo)))+5])
% grid on
% ylabel('Velocidade angular º/s');xlabel('Tempo (s)');
% legend('X','Y')
% 
% % Definição do gráfico 3 (Posição)
% figure(3)
% plot(log_tempo(:,:,modulo),log_pos)
% grid on
% ylabel('Posição (m)');xlabel('Tempo (s)');
% % axis([0 max(log_tempo(:,:,modulo)) min(min(log_pos))-g_const_pos max(max(log_pos))+g_const_pos])
% legend('X_m_e_d_2_0','Y_m_e_d_2_0','Z_m_e_d_2_0');

% Definição do gráfico 4 (Velocidade)
% figure(4)
% plot(log_tempo(:,:,modulo),log_vel)
% grid on
% ylabel('Velocidade (m/s)'),xlabel('Tempo (s)');
% axis([0 max(log_tempo(:,:,modulo)) min(min(log_vel))-g_const_vel max(max(log_vel))+g_const_vel])
% legend('X','Y','Z');

% % Definição do gráfico 5 (Aceleração)
% figure(5)
% plot(log_tempo(:,:,modulo),log_acel)
% grid on
% ylabel('Aceleração (m/s²)'),xlabel('Tempo (s)');
% axis([0 max(log_tempo(:,:,modulo)) min(min(log_acel))-g_const_acel max(max(log_acel))+g_const_acel])
% legend('X','Y','Z');
% 
% % Definição do gráfico 6 (Posição angular)
figure(6)
plot(log_tempo(:,:,modulo),log_pos_ang)
grid on
ylabel('Posição angular (º)'),xlabel('Tempo (s)');
% axis([0 max(log_tempo(:,:,modulo)) min(min(log_pos_ang))-g_const_pos_ang max(max(log_pos_ang))+g_const_pos_ang])
legend('X','Y');

% % Definição do gráfico 7 (Aceleração média)
% media_pond=(sum(log_acel(2:end,:).*([log_tempo_inst(2:end,1) log_tempo_inst(2:end,1) log_tempo_inst(2:end,1)])))/sum(log_tempo_inst(2:end,1));
% figure(7)
% media=[media_pond
%        0 0 0
%        0 0 0];
% media=ones(size(log_tempo_min(:,:,modulo),2),3)*media; %Calculo da média da aceleração
% plot(log_tempo(:,:,modulo),media)
% grid on
% ylabel('Aceleração média (m/s²)'),xlabel('Tempo (s)');
% axis([0 max(log_tempo(:,:,modulo)) min(min(media))-g_const_media max(max(media))+g_const_media])
% legend('X','Y','Z');
% 
% var(A_g(2:end,:,:))
% mean(A2(2:end,:,:))
% 
% % 
% Fs = 1000;   t = 0:1/Fs:.296;
%        x = log_acel;  % A cosine of 200Hz plus noise
%        pwelch(x,[],[],[],Fs,'twosided'); % Uses default window, overlap & NFFT.
%        
% [MUHAT,SIGMAHAT] = normfit(A(:,:,modulo));
% [N,X]=hist(A(:,1,modulo),30);
% Y = NORMPDF(X,MUHAT(1),SIGMAHAT(1));
% 
% figure(8)
% plot(X,N/(sum(N)*(X(2)-X(1))),X,Y)