% this function parses the serial packets (outputted by serialdecode)
% and computes the effective heading and velocity.

function [heading, velocity, timestamp, errorcount]=omnibikeserialdecode(v,vtimestamp)

lastencoder=0;
errorcount=0;

velocity=[];
heading=[];
timestamp=[];

pos=1;
outpos=1;

firsttime=1;

while(pos<(length(v)-4))
    % wait for synchronization byte
    if (v(pos)~=237)
        pos=pos+1;
        continue;
    end
    
    
    checksum=bitxor(v(pos),v(pos+1));
    checksum=bitxor(checksum,bitxor(v(pos+2),v(pos+3)));
    
    analogvalue=v(pos+1)*256+v(pos+2);
    encoder=v(pos+3);
    
    if (firsttime)
        encoderdiff=0;
        firsttime=0;
    else
        encoderdiff=encoder-lastencoder;
        if (encoderdiff<0)
            encoderdiff=encoderdiff+256;
        end
    end
    
    % discard packets whose checksum wrong and
    % heuristically discard bogus data whose checksum happens to be right
    if (checksum~=v(pos+4) | analogvalue>4096 | encoderdiff>5)
        % but only increment pos by 1 so if we're unsync'd,
        % we have a chance to resynchronize.
        pos=pos+1;
        errorcount=errorcount+1;
        continue;
    end
    
    angle=((analogvalue-2048)/2048)*2*pi;
    
    velocity(outpos)=v(pos+3);
    timestamp(outpos)=vtimestamp(pos);
    heading(outpos)=angle;
    outpos=outpos+1;
    
    lastencoder=encoder;
    pos=pos+5;
end

if 0
    % this function parses the serial packets (outputted by serialdecode)
    % and computes the effective heading and velocity.
    
    %function [heading, velocity, timestamp, errorcount]=omnibikeserialdecode(v, vtimestamp)
    
    lastencoder=0;
    errorcount=0;
    
    velocity=[];
    heading=[];
    timestamp=[];
    
    pos=1;
    outpos=1;
    
    while(pos<(length(v)-4))
        % wait for synchronization byte
        if (v(pos)~=237)
            pos=pos+1;
            continue;
        end
        
        
        checksum=bitxor(v(pos),v(pos+1));
        checksum=bitxor(checksum,bitxor(v(pos+2),v(pos+3)));
        
        analogvalue=v(pos+1)*256+v(pos+2);
        encoder=v(pos+3);
        
        encoderdiff=encoder-lastencoder;
        if (encoderdiff<0)
            encoderdiff=encoderdiff+256;
        end
        
        % discard packets whose checksum wrong and
        % heuristically discard bogus data whose checksum happens to be right
        if (checksum~=v(pos+4) | analogvalue>4096) % | encoderdiff>5)   
            % but only increment pos by 1 so if we're unsync'd,
            % we have a chance to resynchronize.
            pos=pos+1;
            errorcount=errorcount+1;
            continue;
        end
        
        angle=((analogvalue-2048)/2048)*2*pi;
        
        velocity(outpos)=v(pos+3);
        timestamp(outpos)=vtimestamp(pos);
        heading(outpos)=angle;
        outpos=outpos+1;
        
        lastencoder=encoder;
        pos=pos+5;
    end
    
end