function pir_scope()

global time;
global plot_handles;
global plot_data;
global subplot1;
global subplot2;
global subplot3;
global comp;

global compass_calib;
global compass_error;

compass_calib = [-86 46; 78 -4];
compass_error = (compass_calib(1,1:end) + compass_calib(2, 1:end))/2;

global samples;

samples = 5;
comp = [];

hold off;
figure(1);

time = 0;
plot_handles = [];
plot_data = [];
s = serial('COM1','BaudRate',9600,'Parity','none');

fopen(s)
s.BytesAvailableFcnMode = 'terminator';
s.BytesAvailableFcn = @serial_in;

while 1
  string = input('S >> ', 's');
  
  fprintf(1,'\n');
  switch string
    case 'quit'
      break
    otherwise
      fprintf(1, 'Invalid command\n');
  end
end

fclose(s)
delete(s)
clear s

delete(comp);
delete(plot_handles);

function serial_in(obj, event)

global time;
global plot_data;
global real_data;
global plot_handles;
global comp;
global samples;
global subplot1;
global subplot2;
global subplot3;
global compass_error;

data = fgets(obj);

%fprintf(1, '%s\n', data); 

time = time + 1;

n = 1;
dirs = [];
for i=1:length(data)
  if data(i) == ':' 
    dirs = [dirs; sscanf(data(n:(i-1)), '%d')];
    n = i+1;
  end
end
dirs = [dirs; sscanf(data(n:(i-1)), '%d')];

if length(dirs) ~= 6
  dirs = [0; 0; 0; 0; 0; 0];
end

%dirs(find(dirs(1:3) > 2050)) = 0;
%dirs(find(dirs(1:3) < -2050)) = 0;
%dirs(find(dirs(4:6) > 32768)) = 0;
%dirs(find(dirs(4:6) < -32768)) = 0;

X_e = dirs(4);% - compass_error(1);
Y_e = dirs(5);% - compass_error(2);

Heading = atan2(Y_e, X_e);

%dirs(1) = (Heading+pi)/(2*pi)*360;
 
plot_data = [plot_data, dirs];

if mod(time, samples) == 0
  if time > 1000
    time_vector = (time-999):time;
    plot_data = plot_data(1:end, (end-999):end);
  else
    time_vector = 1:time;
  end
  
  if length(plot_handles) == 0
    %axes('position',[0.1  .6  0.6  .4]);
    subplot1 = subplot(2,2,1);
    plot_handles = plot(time_vector, plot_data(1:3,1:end));
    
    %axes('position',[0.1  0.1  0.8  .4]);
    subplot2 = subplot(2,2,[3 4]);
    plot_handles = [plot_handles plot(time_vector, plot_data(4:6, 1:end))];
  else
    set(plot_handles(1), 'XData', time_vector, 'YData', plot_data(1, 1:end));
    set(plot_handles(2), 'XData', time_vector, 'YData', plot_data(2, 1:end));
    set(plot_handles(3), 'XData', time_vector, 'YData', plot_data(3, 1:end));
    
    set(plot_handles(4), 'XData', time_vector, 'YData', plot_data(4, 1:end));
    set(plot_handles(5), 'XData', time_vector, 'YData', plot_data(5, 1:end));
    set(plot_handles(6), 'XData', time_vector, 'YData', plot_data(6, 1:end));
    
    refreshdata(subplot1);
    set(1,'CurrentAxes',subplot1)
    ylim([-2050 2050]);
    %ylim([0 360]);
    xlim([time-999 time]);
    
    refreshdata(subplot2);
    set(1,'CurrentAxes',subplot2)
    ylim([-250 250])
    xlim([time-999 time]);
  end
end

rot = [cos(Heading) -sin(Heading); sin(Heading) cos(Heading)];
%  arrow = [ 
%      0 1 0.8 1 0.8;
%      0 0 0.08 0 -0.08
%      ];
%arrow = [
%    0    1    -0.08  0   -0.08
%    0.08 0    -0.08  0   0.08
%];
%arrow = rot * arrow;
arrow = [];
arrow(:,:,1) = [
    0 1 0;
    0.08 0 -0.08;
    1 1 1
];

arrow(:,:,2) = [
    0 -1 0;
    0.08 0 -0.08;
    1 1 1
];

arrow(1:2,:,1) = rot * arrow(1:2,:,1);
arrow(1:2,:,2) = rot * arrow(1:2,:,2);

x(:,:) = arrow(1,:,:);
y(:,:) = arrow(2,:,:);
z(:,:) = arrow(3,:,:);
  
if length(comp) == 0
  subplot3 = subplot(2,2,2);
  %compass = plot([0 0; cos(Heading) sin(Heading)]);
  comp = compass(cos(Heading), sin(Heading));
  hold on;
  delete(comp);
  %comp = patch(arrow(1,:), arrow(2,:), [1 1 1 1 1]);
  tcolor(1,1,1:3) = [0 0 0];
  tcolor(1,2,1:3) = [1 0 0];
  comp = patch(x,y,z,tcolor);
else
  set(comp, 'XData', x, 'YData', y);
  refreshdata(subplot3);
end
  
hold on;
