interp_imu_v000

%%%C-Type implementation for imu interpolation for iphone data
%oper=new output period
%cdef= col definitions [total, Time , x , y, z]
function interp_imu(ACCFILE, GYROFILE, OUTFILE, cdef, oper)

% fclose all;
% clear;
% DIRNAME='C:\Documents and Settings\Yigiter\Desktop\data\CalibTest1\';
% INFILES=[[DIRNAME '07-16-00-12-acclog.bin ']; [DIRNAME '07-16-00-12-gyrolog.bin']];
% OUTFILE=[DIRNAME 'imu0.bin'];
% cdef=[5 2 3 4 5;5 2 3 4 5];
% oper=2;
% ACCFILE=INFILES(1,:);
% GYROFILE=INFILES(2,:);

%file parameters
ARN=cdef(1,1);
GRN=cdef(2,1);
ATR=cdef(1,2);
GTR=cdef(2,2);
AAR=cdef(1,3:end);
GAR=cdef(2,3:end);

%i/o files
afid=fopen(ACCFILE,'rb');
gfid=fopen(GYROFILE,'rb');
ofid=fopen(OUTFILE,'wb');


%output period
np=oper;    %new period

%first data (useless)
adat=fread(afid,[ARN 1],'double');
gdat=fread(gfid,[GRN 1],'double');

if (gdat(GTR)>adat(ATR))
    ct=gdat(GTR);
    while (adat(ATR)<ct)
        adat_p=adat;
        adat=fread(afid,[ARN 1],'double');
    end
    gdat_p=gdat;
    gdat=fread(gfid,[GRN 1],'double');
else
    ct=adat(ATR);
    while (gdat(GTR)<ct)
        gdat_p=gdat;
        gdat=fread(gfid,[GRN 1],'double');
    end
    adat_p=adat;
    adat=fread(afid,[ARN 1],'double');
end
oin=0;
fwrite(ofid, [oin;adat_p(AAR);gdat_p(GAR)],'double');

adt=0;
gdt=0;
apt=ct;
gpt=ct;
ginc=zeros(length(GAR),1);
ainc=zeros(length(AAR),1);
gflag=0;
aflag=0;
while (~(feof(gfid) || feof(afid)))
    %Gyroscope step
    if (~gflag)
        dt=gdat(GTR)-gpt;
        if ((gdt+dt<=np))
            ginc=ginc+dt*gdat(GAR,1);
            gdt=gdt+dt;

            gpt=gdat(GTR);
            gdat=fread(gfid,[GRN 1],'double');
        else
            ginc=ginc+(np-gdt)*gdat(GAR,1);
            gpt=gpt+(np-gdt);
            gflag=1;
        end
    end
    
    
    %Acc step
    if (~aflag)
        dt=adat(ATR)-apt;
        if (adt+dt<=np)
            ainc=ainc+dt*adat(AAR,1);
            adt=adt+dt;

            apt=adat(ATR);
            adat=fread(afid,[ARN 1],'double');
        else
            ainc=ainc+(np-adt)*adat(AAR,1);
            apt=apt+(np-adt);
            aflag=1;
        end
    end
    
    
    if (aflag && gflag)
        %%Write data
        oin=oin+1;
        fwrite(ofid, [oin;ainc/np;ginc/np],'double');
        
        %Prepare for the next cycle
        aflag=0;
        gflag=0;
        
        adt=0;
        gdt=0;
        
        ainc=zeros(length(AAR),1);
        ginc=zeros(length(GAR),1);
    end
end

fclose(afid);
fclose(gfid);
fclose(ofid);

return;
% %%Check results;
% clear;
% 
% acc=readbin_v000('other\06-12-39-56-acclog.bin',5);
% gyro=readbin_v000('other\06-12-39-56-gyrolog.bin',5);
% out=readbin_v000('other\06-12-39-56-imu.bin',7);
% 
% g1=zeros(4,length(gyro));
% for (i=2:length(gyro))
%     dt=gyro(2,i)-gyro(2,i-1);
%     g1(1,i)=g1(1,i-1)+dt;
%     g1(2:4,i)=g1(2:4,i-1)+gyro(3:5,i)*dt;
% end
% 
%     
% g2=zeros(4,length(out));
% dt=1/20;
% for (i=2:length(out))
%     g2(1,i)=g2(1,i-1)+dt;
%     g2(2:4,i)=g2(2:4,i-1)+out(5:7,i)*dt;
% end