function [time_array]=fun_to_delay(fun)
%fun_to_delay allows you to input a function handel and get out the array of
%   delay times to send to the PIC
%fun is a function handel you want the delay times of.  Fun should output
%   displacement when time is inputted.  Time for the function you want
%   should start at 0 and start off by going at positive values, since it
%   ends when it goes from a negative value to a positive value.  The
%   output should be in mm, and the input should be in seconds.
%time_array is a signed output of delay times.  The magnitude represents
%   the direction, while the magnitude represents the magnitude of delay in
%   us

%set parameters
diameter=1.783*25.4; %mm
circumfrance=pi*diameter;
steps_per_rev=360/1.8;
step_dist=circumfrance/steps_per_rev;

%initialize iteration values
last_t=0;
cur_t=0;
dist_new=fun(0/1000000);
dist_old=fun(0/1000000);
iter=1;
step_num=1;
total_dist=step_dist*2;

%figure out lengths for steps throught itterating integration of function
while ~(total_dist<=0)
    last_t=cur_t;
    dist_new2=dist_old+step_dist;

    %need to have inital total distance of a non-zero number for while loop
    %   to start iterating
    if iter==1 
        total_dist=0;
    end
    
    %find the change in t to get step distance
    while abs(dist_new-dist_old)<step_dist
        cur_t=cur_t+1;
        dist_new=fun(cur_t/1000000);
        
        %when approaching end point, make sure it reaches it
        if abs(total_dist)<step_dist*1.01 & dist_new2<dist_new & iter > 2
            break; %approached origion
        elseif abs(total_dist)<step_dist*1.01
            dist_new2=dist_new; %approaching origion
        end
        
    end
    
    %keep track in change and time and set negative sign if goes backward
    time_array(iter)=cur_t-last_t;
    if dist_new-dist_old<0
        time_array(iter)=-time_array(iter);
    end
    
    %keep track of change in distance
    total_dist=total_dist+sign(dist_new-dist_old)*step_dist;
    dist_old=dist_old+sign(dist_new-dist_old)*step_dist;
    
    %display current distance
    fprintf('Current Distance = %3.2f mm\n',total_dist);
    
    %iterate to next value
    iter=iter+1;
    step_num=step_num+1;
end

%make sure net distance of one cycle is 0
while total_dist>step_dist/2
    time_array(liter)=-abs(time_array(iter-1))
    total_dist=total_dist-step_dist;
    iter=iter+1;
end
while total_dist<-step_dist/2
    time_array(iter)=abs(time_array(iter-1))
    total_dist=total_dist+step_dist;
    iter=iter+1;
end
fprintf('Finished creating array of time delays\n');