%Vehicle Parameterrs
Nw=2;
f=0.01;
Iz=2667;
a=1.35;
b=1.45;
By=0.27;
Cy=1.2;
Dy=0.7;
Ey=-1.6;
Shy=0;
Svy=0;
m=1400;
g=9.806;


%note constraints on input
%delta [-0.5, 0.5]
%Fx [-5000, 5000]

init = [287, 5, -176, 0, 2, 0];
curr_xy = [287, -176];
U_try = [0.4, 5000];

for i = 1:12
    U_try = [U_try; U_try];
end
U_final = [];
U_try = [0.4, 5000; 0.4, 5000];
[Y, T] = forwardIntegrateControlInput(U_try);
traj_inf = getTrajectoryInfo(Y, U_try);
%traj_inf.t_finished

init = [287, 5, -176, 0, 2, 0];
U_try = [0.25, 2500; 0.25, 2500; 0.25, 2500];
noise = [randn*0.01, randn*10];
U_try = U_try + noise;
[Y, T] = forwardIntegrateControlInput(U_try);
traj_inf = getTrajectoryInfo(Y, U_try);
U_final = U_try;

max_percent = 0; 
num_iter = 0;
subt = 0;
randomize = 0;
percent_comp = 0;
avgcount = [];
while (size(traj_inf.t_finished) == 0)
%for i = 1:100
    noise = [randn*0.01, randn*10];
    U_try = U_try + noise;
    [Y_temp, T] = forwardIntegrateControlInput(U_try, Y(end,:));
    traj_inf = getTrajectoryInfo([Y;Y_temp], [U_final; U_try]);
   
    
    if mod(num_iter+1, 11) == 0 | traj_inf.percent_of_track_completed < (percent_comp - 0.0001)
        randomize = 1;
    end
    %if crashes, redo, if not add on and keep going 
    %also will allow it to change inputs once in a while for randomization
    
    %next thing to onsider trying is the intersectLineSegment thing and
    %following centerline
    count = 1;
    factor = 3;
    while size(traj_inf.left_track_position) ~= 0 | randomize == 1
        if subt == 0        
            U_og = U_try;
            U_try = U_try + [0.011, 250];
            if (U_try(1,1) > 0.49 && U_try(1,2) > 4900)
                subt = 1;
            end
            if U_try(1,1) > 0.45 
                U_try = U_try - [0.05, 0];
            end
            if U_try(1,2) > 4500
                U_try = U_try - [0, 660];
            end
            noise = [randn*0.01, randn*10];
            U_try = U_try + noise;
        end
        
        if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700)
            subt = 1;
            [Y_comp1, T] = forwardIntegrateControlInput(U_try, Y(end,:));
            U_try = U_og;
        end

        
        if subt == 1  
            if (U_try(1,1) < -0.49 && U_try(1,2) < -4900)
                subt = 0;
            end
            U_try = U_try - [0.011, 250];
            if U_try(1,1) < -0.45 
                U_try = U_try + [0.05, 0];
            end
            if U_try(1,2) < -4500
                U_try = U_try + [0, 660];
            end
            noise = [randn*0.01, randn*10];
            U_try = U_try + noise;
        end
        
        if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700)
            [Y_comp2, T] = forwardIntegrateControlInput(U_try, Y(end,:));
            
            
            for i = 1:(246/3)                
                dist1 = norm([Y_comp1(:,1)'; Y_comp1(:,3)'] - TestTrack.cline(:,i:i+2));
                dist2 = norm([Y_comp2(:,1)'; Y_comp2(:,3)'] - TestTrack.cline(:,i:i+2));
                            
                diff1 = min(dist1);
                diff2 = min(dist2); 
                
                if diff1 < 20 | diff2 < 20 
                    break
                end
            end
            
            if diff1 > diff2
                subt = 0;
            end
            if diff2 > diff1
                subt = 1;
            end
            
            U_try = U_og;
            if subt == 0       
                U_try = U_try + [0.011, 250];
%                 if (U_try(1,1) > 0.49 && U_try(1,2) > 4900)
%                     subt = 1;
%                 end
                if U_try(1,1) > 0.5 
                    U_try = U_try - [0.05, 0];
                end
                if U_try(1,2) > 4000
                    U_try = U_try - [0, 760];
                end
            end


            if subt == 1  
%                 if (U_try(1,1) < -0.49 && U_try(1,2) < -4900)
%                     subt = 0;
%                 end
                U_try = U_try - [0.011, 250];
                if U_try(1,1) < -0.5 
                    U_try = U_try + [0.05, 0];
                end
                if U_try(1,2) < -4000
                    U_try = U_try + [0, 760];
                end

            end
        end
        
        %instead we'll go back one step and so we'll send 6 commands and
        %check
        
        if mod(count,14) == 0
            factor = factor + 3;
        end
        
        if factor > 5
            enteredifstatementon166 = 1
            break 
            break
        end
        
        U_try(:,1) = min(max(U_try(:,1), -0.5), 0.5);
        U_try(:,2) = min(max(U_try(:,2), -5000), 5000);
        U_try2 = U_try;
        
        for j = 1:(factor/3)
            U_try2 = [U_try2; U_try];
        end
        
        [Y_temp, T] = forwardIntegrateControlInput(U_try2, Y(end-factor,:));
        traj_inf = getTrajectoryInfo([Y(1:end-factor,:);Y_temp], [U_final(1:end-factor,:); U_try2]);
        
        if size(traj_inf.left_track_position) == 0
            U_final = U_final(1:end-factor,:);
            Y = Y(1:end-factor,:);
        end
        count = count + 1;
        randomize = 0;
    end
    
    if count ~= 1
        avgcount = [avgcount ; count];
    end
    
    if (traj_inf.percent_of_track_completed > percent_comp)
        percent_comp = traj_inf.percent_of_track_completed;
    end
    
    U_final = [U_final; U_try];
    Y = [Y; Y_temp(1:3,:)];
    num_iter = num_iter + count
end