close all
clear all
clc

database.timesample=0;
database.mod.sensor.scale=0;
database.mod.sensor.data=zeros(3,1);

buf0=zeros(23,1);
log=zeros(1,2);
rawacclog=zeros(1,3,3);
rawacccntr=ones(3,1);
rawgyrolog=zeros(1,2,3);
rawgyrocntr=ones(3,1);
anglelog=zeros(1);
rolllog=zeros(1);

angulo =0;
size=1000;

% dt = 0.1;
% cutoff=2*pi; %cutoff freq = 1/5 Hz

angulovec=zeros(2,1);
last=zeros(3,3,3);
lastgyro=zeros(3,2);

figure(1);
H=bar3(last(:,:,3));
axis([0 4 0 4 -33000 33000]);
xlabel('X   Y   Z');
ylabel('Mod1 Mod2 Mod3');
zlabel('Aceleration');

figure(2);
K=bar3(lastgyro);
axis([0 4 0 4 -300 300]);
xlabel('X   Y   Z');
ylabel('Mod1 Mod2 Mod3');
zlabel('Angular rate');

figure(2);

SerialHandle=serial('com9','Baudrate',19200,'Databits',8,'StopBits',1);

fopen(SerialHandle);
u=0;
 counter=1;
 buf=zeros(21:1);
 while(1)%counter<=size+1)

a=0;
while(a==0)
    
%     %alternativa
%     buf(1)=fread(SerialHandle,1,'uint8');
%     if buf(1)==170
%         buf(2)=fread(SerialHandle,1,'uint8');
%         if buf(1)==170 && buf(2) == 85
%             for g=3:21
%                 buf(g)=fread(SerialHandle,1,'uint8');
%             end
%         end
%     end
%       
%     for h=3:20
%         if buf(h)==170 && buf(h+1)==85
%            buf=0; 
%         else
%             
%             timetag=buf(3:5);
%             info1=buf(6:7);
%             data1=buf(8:13);
%             info2=buf(14:15);
%             data2=buf(16:21);
%     
%     %end alternativa


[start,count]=fread(SerialHandle,2,'uint8');
if(count==2 && start(1)==170 && start(2)==85)
%     if u==0
%         tic
%         u=1;
%     else
%         toc
%         u=0;
%     end
    a=1;
   [timetag,count2]=fread(SerialHandle,3,'uint8');
   [info1,count3]=fread(SerialHandle,2,'uint8');
   data1=fread(SerialHandle,3,'int16');
   %rawlog(counter,:)=data1';
   [info2,count5]=fread(SerialHandle,2,'uint8');
   [data2,count6]=fread(SerialHandle,3,'int16');
   %rawlog(counter,:)=data2';
%    uint8(timetag);
%    uint8(info1);
%    uint8(data1);
%    uint8(info2);
%    uint8(data2);
   
   nfo=getnfo(info1);
   
   timestmp=cnv32(timetag);
   
   if timestmp~=database(counter).timesample
       counter=counter+1;
   end
   database(counter).timesample=timestmp;
%    database(counter).mod(nfo(1)).sensor(nfo(2)).scale=nfo(3);
%    database(counter).mod(nfo(1)).sensor(nfo(2)).data=getdata(data1);
%     last(nfo(1),:,3)=(cutoff^2 .* (data1' - last(nfo(1),:,2)).*dt^2 -sqrt(2).*cutoff.*(last(nfo(1),:,2)-last(nfo(1),:,1)).*dt) + 2.*last(nfo(1),:,2) - last(nfo(1),:,1); %butterworth
%     last(nfo(1),:,1)=last(nfo(1),:,2);
%     last(nfo(1),:,2)=last(nfo(1),:,3);
%     last(nfo(1),:,3)=data1';
   last(nfo(1),:,3)=data1';
   %last(nfo(1),:)=(last(nfo(1),:)+getdata(data1)')./2;
%    nfo=getnfo(info2);
%    database(counter).mod(nfo(1)).sensor(nfo(2)).scale=nfo(3);
%    database(counter).mod(nfo(1)).sensor(nfo(2)).data=getdata(data2);
%    lastgyro(nfo(1),1:2)=database(counter).mod(nfo(1)).sensor(nfo(2)).data(1:2);
    lastgyro(nfo(1),1:2)=data2(1:2)';
       
       rawacccntr(nfo(1))=rawacccntr(nfo(1))+1;
       
       rawgyrocntr(nfo(1))=rawgyrocntr(nfo(1))+1;
   
   %figure(1);
   setbardata(last(:,:,3),H,3);
   %figure(2);
   setbardata(lastgyro,K,2);
   
   pause(0.01)
   
   if 1%last(3,1,3)>last(2,1,3)-3000 && last(3,1,3)<last(2,1,3)+3000 %&& last(1,1,3)>last(2,1,3)-3000 && last(1,1,3)<last(2,1,3)+3000
      %angulo=(angulo.*0.7+(acosd(last(3,3,3)/last(2,3,3))).*0.3);%+(asind(last(3,2)/last(2,3))))/2
      %vpa(angulo,2)
      angulo2=vrrotvec([0 last(2,2:3,3)],[0 last(3,2:3,3)]);
      angulo22=vrrotvec(last(2,:,3),last(3,:,3));
      %angulo3=vrrotvec(last(2,:,3),last(1,:,3));
      angulovec(1)=angulovec(1).*0+(angulo2(4)/pi)*180*1;
      anglelog(counter)=(angulo2(4)/pi)*180;
      %angulovec(2)=angulovec(2).*0+(angulo3(4)/pi)*180*1;
      angulovec'
      %log(counter,:)=angulovec';
      rolllog(counter)=(atan2(angulo22(3),angulo22(1))*180)/pi;
      
      rawacclog(counter,:,:)=last(:,:,3)';
      rawgyrolog(counter,:,:)=lastgyro(:,:)';
      
      counter
      
   end
   timetag(:)=[0;0;0;];
info1(:)=[0;0];
data1(:)=[0;0;0];
info2(:)=[0;0];
data2(:)=[0;0;0];
        end


    end


end
%   end

fclose(SerialHandle);
