clc
close all;
clear all;

dt = 1;
t=[0];
% m=[0;0;0;0]
m=[0;0;0;0;0;0;0;0;];%温庭玮+
p = plot(t,m);%温庭玮+
Total_step_left = 0;%温庭玮+
Total_step_right = 0;
pre_num = 0;%温庭玮+
pre_num2 = 0;
num2 = 0;
step = 0;
up_flag1 = 0;
RK = 10000;
LK = 10000;
acc_flag = 0;
step2 = 0;
up_flag2 = 0;
TimingMark = 0;
down_step = 0;
Total_Down_Step = 0;
%global counter;
%counter = 0;
global front_flag;
global back_flag;
front_flag = 0;
back_flag = 0;
global front_flag2;
global back_flag2;
front_flag2 = 0;
back_flag2 = 0;
global counter;
counter=0;
global counter2; 
counter2 = 0;
global accl;
global accl2;
accl = 0;
accl2 = 0;
%{
global FrontLiftPoint2;
FrontLiftPoint2=0;
global BackLiftPoint2;
BackLiftPoint2=0;
global FrontDownPoint2;
FrontDownPoint2=0;
global BackDownPoint2;
BackDownPoint2=0;
%}
global FrontHighPoint;
global FrontLowPoint;
global BackHighPoint;
global BackLowPoint;
FrontHighPoint = 10000;
FrontLowPoint = 10000;
BackHighPoint = 10000;
BackLowPoint = 10000;
data = [];
buf = [];
x=0;
%legend('左脚气压','右脚气压','r2','r3','b0','b1','b2','b3')
legend('right_front_mag','right_back_mag','left_front_mag', 'left_back_mag','h_zpt','down','zupt')
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 = [];

is_down = 0;
is_up = 0;
is_left = 0;
is_right = 0;
 
count = 0;

mag_last = [0;0;0];

transfer =[0 -1 0; -1 0 0; 0 0 1];
press = zeros(1,18);
gyr_data = zeros(1, 3);
mag_data = zeros(1, 3);

rotate_matrix = eye(3);

right_pos_data = [0,0,0];
right_att_data = [0,0,0];
left_pos_data  = [0,0,0];
left_att_data  = [0,0,0];

down = 0;
jump = 0;

s_rssi = 0;

rssi_buff =[];
last_rssi = 0.1;

time_stamp = 0;

pfcount = 0;
pfTime = 0;

deltaPos = [0;0;0];
lastPos = [0;0;0];

particleNumber = 500;
lastWeight = ones(1,particleNumber)/(particleNumber);
lastParticle = zeros(particleNumber, 2) + 0.1 *(randn(particleNumber, 2));

pos_time = 0;
att_time = 0;

traj_rotate_matrix = [1, 0; 0, 1];
right_pos_buf = [];
last_s_zupt = 1;
s_zupt = 1;
h_zupt = 1;
last_h_zupt = 1;

right_global_pos = [0;0;];
delta_pos = [0,0];
time_stam = 0;
acc_data = [];
mag_data = [];

h_acc_buff = [];
h_zupt_last = 1;
s_acc_buff = [];

s_zupt_count = 0;
h_zupt_count = 0;

max_rssi_val = 0;
min_rssi_val = 100;

rssi_buff = [];

left_press = 0;
right_press = 0;

zupt_count = 0;
left_acc_norm = 0;
right_acc_norm = 0;

left_press_buff = [];
right_press_buff = [];

last_jump = 0;

left_last_z = 0;
right_last_z = 0;

s_zupt_last = 0;

press_wait_count = 0;

right_data_norm_buff = [];
acc_wait_count = 0;

right_data_x_buff = [];
right_data_y_buff = [];
right_data_z_buff = [];
left_data_z_buff = [];

zero_x_switch = 0;
zero_y_switch = 0;
zero_z_switch = 0;

zero_x_wait = 0;
zero_y_wait = 0;
zero_z_wait = 0;

z_raised = 0;
z_hollow = 0;

x_raised = 0;
x_hollow = 0;

y_raised = 0;
y_hollow = 0;

last_show_z = 0;
last_show_y = 0;
last_show_x = 0;

poly_val_3 = [ -0.4484   -0.3138   -0.2030   -0.1159   -0.0525   -0.0128    0.0031   -0.0047   -0.0362];
poly_val_3 = [-0.4657   -0.3077   -0.1806   -0.0844   -0.0191    0.0153    0.0188   -0.0087   -0.0670];%慢走

acc_wait_cout = 0;

left_median_buff=[3000,3000,3000,3000,3000];
right_median_buff=[3000,3000,3000,3000,3000];

left_acc_buff = [];
right_acc_buff = [];

step_count = -1;

right_front_mag = 0;
right_back_mag = 0;

left_front_mag = 0;
left_back_mag = 0;

left_front_mag_buff = [];
right_front_mag_buff = [];

left_back_mag_buff = [];
right_back_mag_buff = [];

right_acc_z_buff = [];
left_acc_z_buff = [];

cancel_down = 0;
last_down = 0;

left_on_floor = 1;
right_on_floor = 1;

virus_flag = 0;

special_front_down  = 0;

real_max_val_right = 0;
real_max_val_left = 0;

up_flag = 0;

right_back_stage = 0;
left_back_stage = 0;

left_acc_buff =[];
right_acc_buff =[];

special_down_count_down = 0;

left_up_trend = 0;
right_up_trend = 0;

% for i=1:shuliang
while true
    data=fread(object,320);%读取数据
    while ~isempty(data)
        buf = [buf data(1)];
        data(1) = [];
        switch state
            case 0
               if length(buf)>=5
                   if  buf(1)==170
                       Len = buf(2);
                       Lenf = 255 - buf(2);
                       if buf(3) == 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(4) == 4
                          disp(buf(Len));
                           right_pos_data(1) = double(bitshift(int16(buf(5)),24)+bitshift(int16(buf(6)),16)+bitshift(int16(buf(7)),8)+bitshift(int16(buf(8)),0))/100;
                           right_pos_data(2) = double(bitshift(int16(buf(9)),24)+bitshift(int16(buf(10)),16)+bitshift(int16(buf(11)),8)+bitshift(int16(buf(12)),0))/100;
                           right_pos_data(3) = double(bitshift(int16(buf(13)),24)+bitshift(int16(buf(14)),16)+bitshift(int16(buf(15)),8)+bitshift(int16(buf(16)),0))/100;
                           
                           left_pos_data(1) = double(bitshift(int16(buf(17)),24)+bitshift(int16(buf(18)),16)+bitshift(int16(buf(19)),8)+bitshift(int16(buf(20)),0))/100;
                           left_pos_data(2) = double(bitshift(int16(buf(21)),24)+bitshift(int16(buf(22)),16)+bitshift(int16(buf(23)),8)+bitshift(int16(buf(24)),0))/100;
                           left_pos_data(3) = double(bitshift(int16(buf(25)),24)+bitshift(int16(buf(26)),16)+bitshift(int16(buf(27)),8)+bitshift(int16(buf(28)),0))/100;
                           
                           right_att_data(1) = double(bitshift(int16(buf(29)),8)+bitshift(int16(buf(30)),0));
                           right_att_data(2) = double(bitshift(int16(buf(31)),8)+bitshift(int16(buf(32)),0));
                           right_att_data(3) = double(bitshift(int16(buf(33)),8)+bitshift(int16(buf(34)),0));

                           left_att_data(1) = double(bitshift(int16(buf(35)),8)+bitshift(int16(buf(36)),0));
                           left_att_data(2) = double(bitshift(int16(buf(37)),8)+bitshift(int16(buf(38)),0));
                           left_att_data(3) = double(bitshift(int16(buf(39)),8)+bitshift(int16(buf(40)),0));
                           
                           yaw = left_att_data(1);
                           pitch = left_att_data(2);
                           roll = left_att_data(3);
                           
                          rotate_matrix= [cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+ cos(yaw)*sin(pitch)*cos(roll);
                          sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(yaw)*sin(pitch)*sin(roll), -cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)* cos(roll);
                          -sin(pitch), cos(pitch)* sin(roll), cos(pitch)*cos(roll)];
                           
                           right_acc_data(1) = double(bitshift(int16(buf(41)),8)+bitshift(int16(buf(42)),0));
                           right_acc_data(2) = double(bitshift(int16(buf(43)),8)+bitshift(int16(buf(44)),0));
                           right_acc_data(3) = double(bitshift(int16(buf(45)),8)+bitshift(int16(buf(46)),0));
                            
                           %right_acc_buff = [right_acc_buff,[right_acc_data(1);right_acc_data(2);right_acc_data(3)]./2048];
                           right_acc_buff = [right_acc_buff,[right_acc_data(1);right_acc_data(2);right_acc_data(3)]];
                           
                           left_acc_data(1) = double(bitshift(int16(buf(47)),8)+bitshift(int16(buf(48)),0));
                           left_acc_data(2) = double(bitshift(int16(buf(49)),8)+bitshift(int16(buf(50)),0));
                           left_acc_data(3) = double(bitshift(int16(buf(51)),8)+bitshift(int16(buf(52)),0));
                           
                           %left_acc_buff = [left_acc_buff,[left_acc_data(1);left_acc_data(2);left_acc_data(3)]./2048];
                           left_acc_buff = [left_acc_buff,[left_acc_data(1);left_acc_data(2);left_acc_data(3)]];
                            
                           s_zupt = double(bitand(buf(53),8))/8;
                           h_zupt = double(bitand(buf(53),4))/4;
                           down = double(bitand(buf(53),2))/2;
                           jump = double(bitand(buf(53),1))/1;
                           
                           
                           rssi = double(int8(buf(54)));

                           att_time = double(uint8(buf(55)));
                           
                           right_front_mag = double(bitshift(uint16(buf(56)),8)+bitshift(uint16(buf(57)),0));
                           right_back_mag = double(bitshift(uint16(buf(58)),8)+bitshift(uint16(buf(59)),0));
                           
                           left_front_mag = double(bitshift(uint16(buf(60)),8)+bitshift(uint16(buf(61)),0));
                           left_back_mag = double(bitshift(uint16(buf(62)),8)+bitshift(uint16(buf(63)),0));
                           
                           left_front_mag_buff = [left_front_mag_buff, left_front_mag];
                           right_front_mag_buff = [right_front_mag_buff, right_front_mag];
                           
                           left_back_mag_buff = [left_back_mag_buff, left_back_mag];
                           right_back_mag_buff = [right_back_mag_buff, right_back_mag];
                           
                           
                           
                           R_acc = right_acc_buff(3,1:1:end);
                           R_front = right_front_mag_buff(1:1:end);
                           R_back = right_back_mag_buff(1:1:end);
                           num = length( R_front);
                               
                            if (num>10||num==10) && (num>pre_num)
                                Acc(3) = R_acc(num);
                                Acc(2) = R_acc(num-1);
                                Acc(1) = R_acc(num-2);
                           
                                %Acc(2) = R_acc(num-3);
                                %Acc(1) = R_acc(num-4);
                                
                               [step,up_flag1] = func_count5(R_front,R_back,up_flag1,Acc);
                               Total_step_right = Total_step_right + step;
                               k(num) = step*23000;             
                               RFF(num) = up_flag1*15000;
                               FF_R(num) = front_flag2*2000;
                               BF_R(num) = back_flag2*3000;
                               RFC(num) = accl2;
                            end
                           
                           pre_num = num;
                           
                           
                           L_acc = left_acc_buff(3,1:1:end);
                           L_front = left_front_mag_buff(1:1:end);
                           L_back = left_back_mag_buff(1:1:end);
                           
                           num2 = length( L_front);
                          
                            if (num2>10||num2==10) && (num2>pre_num2)
                                Acc(3) = L_acc(num);
                                Acc(2) = L_acc(num-1);
                                Acc(1) = L_acc(num-2);
                           
                                %Acc(2) = L_acc(num2-3);
                                %Acc(1) = L_acc(num2-4);                            
                               [step2,up_flag2] = func_count6(L_front,L_back,up_flag2,Acc);
                               Total_step_left = Total_step_left + step2;
                               k2(num2) = step2*23000;   
                               LFF(num2) = up_flag2*15000;
                               FF(num2) = front_flag*2000;
                               BF(num2) = back_flag*3000;
                               LFC(num2) = accl;
                            end
                           
                           pre_num2 = num2;
                           
                          TS = Total_step_right + Total_step_left;
                           
                       end
                       

                     if(count == 0)
                         count = 0;
                     end
                    
                     

                    dt = dt + 1;
                    t=[t dt];
                    if dt>250 
                        x=x+1; 
                    end
                    
%                     disp([left_press, right_press])
%                      m=[m [  left_pos_data(3) - left_last_z; left_att_data(1)*0.15;  left_att_data(2)*0.15; left_att_data(3)*0.15; h_zupt *0.40;0;0;0; ]];
                    m=[m [  right_front_mag; right_back_mag; left_front_mag; left_back_mag;0;
                        s_zupt * 3500; 0 * 35000;0;]];
                   % m=[m [  right_acc_data(3); 0; 0; 0;0;
                     %   s_zupt * 3500; 0 * 35000;0;]];
                    % m=[m [  right_acc_data(1); right_acc_data(2);right_acc_data(3);left_acc_data(1); left_acc_data(2);left_acc_data(3);0;
                     %   s_zupt * 3500; 0 * 35000;0;]];
                            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(5),'XData',t,'YData',m(5,:))
                            set(p(6),'XData',t,'YData',m(6,:)) 
                            set(p(7),'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,:))
%                             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,:))
                   
                        buf = buf(Len:end);
                   else
                       buf(1) = [];
                   end
                   state = 0;
                end
            otherwise
                state = 0;
        end
    end
    drawnow limitrate nocallbacks
	axis([x x+300 0 30000]); 
end

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