clear
% SETUP of USB AccessPoint

% port_number='/dev/ttyACM0';   
port_number='com8';

s = serial(port_number);
    s.BaudRate=115200;
    s.InputBufferSize = 1024;
    s.FlowControl='none';
    s.Timeout=1;
    s.ReadAsyncMode='continuous'; % !!!!!!! important
        if s.Status(1:4)=='open' disp('Closing formerly opened port');fclose(s);end;

    s.ErrorFcn=@mycallback; % not important - you can specify your own error callback


%  Port open
fopen(s);   % you can use exceptions, but better soluton is break on error
            % when there is still problem after first run - try RESTART MATLAB!!!
    instrfind
    get(s)  % display port status and settings

% record(s,'on');





% Header of FRAMEs settings

    StartAccessPoint=[255, 7,3];    % numbers taken from ez430 souce code

    StopAccessPoint=[255,9,3];

    AccDataRequest=[255,8,7,0,0,0,0];


% Start AccessPoint
fwrite(s,StartAccessPoint);     % after cca 1s green LED must blink

disp(['Please turn your watch to sync mode (key #, then key v - LED blinking), then press enter...']);
disp(['CTRL+C to stop']);
pause();
% Read (and store) data




% Main read cycle

stop_flag=0;
i=0;
n=0;
t=1;
data_pos3 = [];
ind = [];
t1 = clock;
while stop_flag<100
    i=i+1;
    n=n+1;
    try
        fwrite(s,AccDataRequest);
        a=fread(s,7,'int8');
    
        X=a(1);
        Y=a(2);
        Z=a(3);
        %     I don't now why, but sometimes the correct assignment is:
        %     X=a(6);
        %     Y=a(5);
        %     Z=a(7);


        data(i,1)=X;
        data(i,2)=Y;
        data(i,3)=Z;

        
        t2 = clock;
        e = etime(t2,t1);
        
        dados = data;
        if(size(dados,1) > 20)
            for i = 2:size(dados,1)
                if (dados(i,1)==0)
                    dados(i,1) = dados(i-1,1);
                end
                if (dados(i,2)==0)
                    dados(i,2) = dados(i-1,2);
                end
                if (dados(i,3)==0)
                    dados(i,3) = dados(i-1,3);
                end
            end

            dadosFilt=tsmovavg(dados,'s',15,1);

    %       limiar1=30;
            limiar2=45;
            limiar3=140;
            
            SizePos3 = size(data_pos3,1);
    %       [data_pos1 data_picos1]=peakfinder(dadosFilt(:,1),limiar1);
            [data_pos2 data_picos2]=peakfinder(dadosFilt(:,2),limiar2,-1);
            [data_pos3 data_picos3]=peakfinder(dadosFilt(:,3),limiar3);
            
            if(SizePos3 < size(data_pos3,1))
                ind(1,t) = size(data_pos2,1);
                t=t+1;
            end
            data_pos2(ind) = [];
            data_picos2(ind) = [];

            
            Fa = size(dados,1)/e;
            clc;

            fprintf('Frequência de amostragem: %2.3f Hz\n',Fa);
            fprintf('Tempo decorrido: %2.3f s\n',e);
            
            if(size(data_pos2,1)>4)
                deriv=diff(data_pos2);
                tempos=deriv/Fa;
                rpm=60./tempos;
                rpm(:,1)=tsmovavg(rpm(:,1),'s',3,1);
                fprintf('RPM: %2.2f\n',rpm(size(rpm,1),1));
            end
            
            fprintf('Número de braçadas: %d\n',size(data_pos2,1));
            fprintf('Número de cambalhotas: %d\n',size(data_pos3,1));
%         
        end
        
        % stop on idle     
        if ((X==0) & (Y==0) & (Z==0)) stop_flag=stop_flag+1;end;
        if ((X~=0) | (Y~=0) | (Z~=0)) stop_flag=0;end;

    catch
%         disp('RS232 dropout');
    end;
        
end;

t2 = clock;
e = etime(t2,t1);
data(1,4) = e;
fwrite(s,StopAccessPoint);
delete(instrfindall);


figure(1);
subplot(3,1,1);
hold
plot(dadosFilt(:,1) , 'b');
% plot(data_pos1,data_picos1 , 'v','markerfacecolor',[0 1 0]);
subplot(3,1,2);
hold
plot(dadosFilt(:,2) , 'r');
plot(data_pos2,data_picos2 , 'v','markerfacecolor',[0 1 0]);
subplot(3,1,3);
hold
plot(dadosFilt(:,3) , 'g');
plot(data_pos3,data_picos3 , 'v','markerfacecolor',[0 1 0]);


figure(2)
plot(rpm);