clc
close all;
clear all;

dt = 1;
t=[0];
% m=[0;0;0;0]
m=[0;0;0;0;0;0;0]
p = plot(t,m)

data = [];
buf = [];

x=0;
legend('x','y','z','压力')
grid on;
set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7]);

delete(instrfindall('Type','serial'));%清理串口
object = serial('com3','BaudRate',115200);%配置串口
fopen(object);

state = 0;
Len = 0;
Lenf = 0;

Zarr = [];
Zarr(1) = 0;
Zarr_en  = 0;
run = 0;
left = 0;
right = 0;
acc3_b = 0;
acc3_bb = 0;
press_data = [];
press_var = [];
gyr_var = [];
pos_data = [];

imu_data = [];
% for i=1:shuliang
while true
    data=fread(object);%读取数据
    while ~isempty(data)
        buf = [buf data(1)];
        data(1) = [];
        switch state
            case 0
               if length(buf)>=5
                   if  buf(1)==170 && buf(2)==187 && buf(3)==204
                       Len = buf(4);
                       Lenf = 255 - buf(5);
                       if Len==Lenf
                          state = 1;
                       else
                           buf(1) = [];
                       end
                   else
                       buf(1) = [];
                   end
               end
            case 1
                if length(buf)>=Len
                  ver = 0;
                   for i=1:(Len-1)
                       ver = ver + buf(i);
                   end
                   ver = mod(ver,256);
    %                disp(ver);
    %                disp(buf(Len));
                   if ver==buf(Len)
                       if buf(6)==0 && buf(7)==0
%                             acc(1)=double(bitshift(int32(buf(8)),24)+bitshift(int32(buf(9)),16)+bitshift(int32(buf(10)),8)+int32(buf(11)));
%                             acc(2)=double(bitshift(int32(buf(12)),24)+bitshift(int32(buf(13)),16)+bitshift(int32(buf(14)),8)+int32(buf(15)));
%                             acc(3)=double(bitshift(int32(buf(16)),24)+bitshift(int32(buf(17)),16)+bitshift(int32(buf(18)),8)+int32(buf(19)));
%                             acc(1)=double(bitshift(int16(buf(8)),8)+int16(buf(9)));
%                             acc(2)=double(bitshift(int16(buf(10)),8)+int16(buf(11)));
%                             acc(3)=double(bitshift(int16(buf(12)),8)+int16(buf(13)));
%                             disp(acc);
                            
                            gry(1)=double(bitshift(int16(buf(8)),8)+int16(buf(9)));
                            gry(2)=double(bitshift(int16(buf(10)),8)+int16(buf(11)));
                            gry(3)=double(bitshift(int16(buf(12)),8)+int16(buf(13)));
%                             gyr_norm = [gyr_norm norm([gry(1),gry(2),gry(3)])./16.384*pi/180];
                           
                            acc(1)=double(bitshift(int16(buf(14)),8)+int16(buf(15)));
                            acc(2)=double(bitshift(int16(buf(16)),8)+int16(buf(17)));
                            acc(3)=double(bitshift(int16(buf(18)),8)+int16(buf(19)));
%                             disp(gry);
                            
                             press(1)=double(bitshift(int32(buf(20)),24)+bitshift(int32(buf(21)),16)+bitshift(int32(buf(22)),8)+int32(buf(23)));
                             press_data = [press_data press(1)];
%                             disp(press(1));
                            pos_res(1) = double(bitshift(int16(buf(24)),8)+int16(buf(25)));
                            pos_res(2) = double(bitshift(int16(buf(26)),8)+int16(buf(27)));
                            pos_res(3) = double(bitshift(int16(buf(28)),8)+int16(buf(29)));

                            
                            
                            signal_out = int16(buf(30));
                            if(signal_out == 2)
                                 disp("MOTION_JUMP")
                            elseif(signal_out == 3)
                                disp("MOTION_DOWN")
                                max(press_data(end-6:end)) - min(press_data(end-6:end))
                            elseif(signal_out == 4)
                                disp("MOTION_LEFT")
                               
                                
                            elseif(signal_out == 5)
                                disp("MOTION_RIGHT")
                              
                            end
                            
%                             if(norm(pos_data(1:2,end)) > 0.3)
% %                                 pca_filter(pos_data(:,end-9:end));
%                             end
                            
%                              T = [-0.7949   -0.6067;0.6067   -0.7949];
                            T = [1 0; 0 1];
                             temp = T * [pos_res(1);pos_res(2)];
                             pos_res(1) = temp(1);
                             pos_res(2) = temp(2);
                             
                             
%                             adc(1)=double(bitshift(int16(buf(20)),8)+int16(buf(21)));
%                             adc(2)=double(bitshift(int16(buf(22)),8)+int16(buf(23)));
                            
                            
%                             signal_out(1)=double(bitshift(int32(buf(24)),24)+bitshift(int32(buf(25)),16)+bitshift(int32(buf(26)),8)+int32(buf(27)));
%                             signal_out(2)=double(bitshift(int32(buf(28)),24)+bitshift(int32(buf(29)),16)+bitshift(int32(buf(30)),8)+int32(buf(31)));
%                             signal_out(3)=double(bitshift(int32(buf(32)),24)+bitshift(int32(buf(33)),16)+bitshift(int32(buf(34)),8)+int32(buf(35)));
%                              norm([acc(1);acc(2);acc(3);]./4096/1.1992*9.88)
%                             datestr(now,'mmmm dd,yyyy HH:MM:SS.FFF AM')
%                              pos = footPDR(dt, [gry(1);gry(2);gry(3)]./16.384*pi/180, [acc(1);acc(2);acc(3);]./4096/1.1992*9.7833, press(1));
                            
                         
%                              disp(press(1));
                            
                            dt = dt + 1;
                            t=[t dt];
                            if dt>250 
                                x=x+1; 
                            end
                            imu_data = [imu_data [gry(1)/16.384*pi/180;gry(2)/16.384*pi/180;gry(3)/16.384*pi/180;press(1);]];
                            m=[m [gry(1)/16.384*pi/180;gry(2)/16.384*pi/180;gry(3)/16.384*pi/180;(press(1)-9000000)*0.0000005;pos_res(1)/100;pos_res(2)/100;pos_res(3)/100]];
                                 set(p(1),'XData',t,'YData',m(1,:))
                                  set(p(2),'XData',t,'YData',m(2,:))
                                      set(p(3),'XData',t,'YData',m(3,:))
%                                             set(p(4),'XData',t,'YData',m(4,:))
%                                        set(p(1),'XData',t,'YData',m(5,:))
%                                         set(p(2),'XData',t,'YData',m(6,:))
%                                    set(p(3),'XData',t,'YData',m(7,:))
%                                  set(p(8),'XData',t,'YData',m(8,:))
%                                 set(p(9),'XData',t,'YData',m(9,:))
%                                set(p(10),'XData',t,'YData',m(10,:))
                       end
                        buf = buf(Len:end);
                   else
                       buf(1) = [];
                   end
                   state = 0;
                end
            otherwise
                state = 0;
        end
    end
    drawnow
	axis([x x+300 -2 2]); 
end

fclose(object);
delete(object);
clear object;