%% initial conditions
%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]

load("TestTrack.mat");

init = [287, 5, -176, 0, 2, 0];
curr_pos = [init(1);init(3)];


%state_size = tbd;
% z = [init, init, -0.004, 3900]; %for testing purposes
% nsteps = 2;
% [LB, UB] = bounds(nsteps, 1);
% [g,h,dg,dh]=nonlcon(z, Xobs, nsteps);
% [J, dJ] = costfun(z, TestTrack.cline(:,1), TestTrack.theta(1), nsteps);


Nobs = randi([10 25], 1,1);
global Xobs
Xobs = generateRandomObstacles(Nobs);



U_final = [];
Y_final = [];
options = optimoptions('fmincon','SpecifyConstraintGradient',true,...
                        'SpecifyObjectiveGradient',true) ;

load('ROB535_ControlProject_part1_Team3.mat');              

%[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, init);
load('reftrack_info.mat');

load('segments_info.mat');

%% MPC
U_ref = ROB535_ControlProject_part1_input';
Y_ref = Y_submission';
dt = 0.01;

%discretize dynamics
F_zf=b/(a+b)*m*g;
F_zr=a/(a+b)*m*g;

%we are just going to use cornering stiffness to make linear so this derivative
%easier, the vehicle parameter's are close enough to problem 1 hw 2
B=10;
C=1.3;
D=1;
Ca_r= F_zr*B*C*D;
Ca_f= F_zf*B*C*D;

x = @(s,i) Y_ref(s,i);

Ac = @(i) [0, cos(x(5,i)), 0, -sin(x(5,i)), x(2,i)*sin(x(5,i))-x(4,i)*cos(x(5,i)), 0;
         0, (-1/m)*Ca_f*x(2,i)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1;
         0, sin(x(5,i)), 0, cos(x(5,i)), -x(4,i)*sin(x(5,i))+x(2,i)*cos(x(5,i)), 0;
         0, (1/m)*(-Ca_f*x(2,i)^-2 - Ca_r*x(2,i)^-2) - 1, 0, Ca_r/m*(-1/x(2,i)) + Ca_f/m*(-1/x(2,i)), 0, Ca_r/m*(b/x(2,i)) + Ca_f/m*(-a/x(2,i)) - x(2,i);
         0, 0, 0, 0, 0, 1
         0, (1/Iz)*(-Ca_f*a*x(2,i)^-2 - b*Ca_r*x(2,i)^-2), 0, -b*Ca_r/Iz*(-1/x(2,i)) + a*Ca_f/Iz*(-1/x(2,i)), 0, -b*Ca_r/Iz*(b/x(2,i)) + a*Ca_f/Iz*(-a/x(2,i))];
     
Bc = @(i) [0, -Ca_f/m, 0, Ca_f/m, 0, a*Ca_f/Iz; 
     0, Nw/m, 0, 0, 0, 0]';
 
A = @(i) eye(6) + dt*Ac(i);
B = @(i) dt*Bc(i);


%Decision variables
npred = 10;
nstates = 6;
ninputs = 2;
Ndec=(npred+1)*nstates+ninputs*npred;

input_range = [-0.5, 0.5; -5000, 5000];
eY0 = init';
[Aeq_test1,beq_test1]=eq_cons(1,A,B,eY0,npred,nstates,ninputs);
%simulate forward
T = 0:0.01:(size(Y_ref,2)/100);
%final trajectory
Y=NaN(nstates,length(T));

%applied inputs
U=NaN(ninputs,length(T));

%input from QP
u_mpc=NaN(ninputs,length(T));

%error in states (actual-reference)
eY=NaN(nstates,length(T));

%set random initial condition
Y(:,1)=eY0+Y_ref(:,1);

for i=1:length(T)-1
    i
    %shorten prediction horizon if we are at the end of trajectory
    npred_i=min([npred,length(T)-i]);
    
    %calculate error
    eY(:,i)=Y(:,i)-Y_ref(:,i);

    %generate equality constraints
    [Aeq,beq]=eq_cons(i,A,B,eY(:,i),npred_i,nstates,ninputs);
    
    %generate boundary constraints
    [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs);
    
    %cost for states
    Q=[1,1,1,1,1,1];
    
    %cost for inputs
    R=[0.1,0.01];
    
    H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]);
    
    f=zeros(nstates*(npred_i+1)+ninputs*npred_i,1);
    
    [x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub);
    
    %get linearized input
    u_mpc(:,i)=x(nstates*(npred_i+1)+1:nstates*(npred_i+1)+ninputs);
    
    %get input
    U(:,i)=u_mpc(:,i)+U_ref(:,i);
    
    
    %simulate model
    [~,ztemp]=ode45(@(t,z)kinematic_bike(t,z,U(:,i),0),[0 dt],Y(:,i));
    
    %store final state
    Y(:,i+1)=ztemp(end,:)';
end


%% function start

function [start_idx, end_idx] = get_indices(segment_num, num_pts)
    if segment_num == 1
        start_idx = 1;
        end_idx = num_pts(segment_num);
    else
        start_idx = sum(num_pts(1:segment_num-1)) + 1;
        end_idx = sum(num_pts(1:segment_num));
    end
end

%not used
function x0vec = initvec(x0, u0)
    %function used because fmincon needs initial condition to be size of
    %state vector
    %x0 - last row of Y at checkpoint
    %u0 - last row of U at checkpoint
    global nsteps
    x0vec = [];
    for i = 1:nsteps
        x0vec = [x0vec, x0];
    end
    
    %not sure if inputs should be instantiated or not 
    %will instantiate them to previous u 
    for i = 1:nsteps-1
        x0vec = [x0vec, u0]; 
    end
end

%% mpc functions
function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
    %build matrix for A_i*x_i+B_i*u_i-x_{i+1}=0
    %in the form Aeq*z=beq
    %initial_idx specifies the time index of initial condition from the reference trajectory 
    %A and B are function handles above

    %initial condition
    x_initial=x_initial(:);

    %size of decision variable and size of part holding states
    zsize=(npred+1)*nstates+npred*ninputs;
    xsize=(npred+1)*nstates;

    Aeq=zeros(xsize,zsize);
    Aeq(1:nstates,1:nstates)=eye(nstates); %initial condition 
    beq=zeros(xsize,1);
    beq(1:nstates)=x_initial;

    state_idxs=nstates+1:nstates:xsize;
    input_idxs=xsize+1:ninputs:zsize;

    for i=1:npred
        %negative identity for i+1
        Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i):state_idxs(i)+nstates-1)=-eye(nstates);
        
        %A matrix for i
        Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i)-nstates:state_idxs(i)-1)=A(initial_idx+i-1);

        %B matrix for i
        Aeq(state_idxs(i):state_idxs(i)+nstates-1,input_idxs(i):input_idxs(i)+ninputs-1)=B(initial_idx+i-1);
    end
end

function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs)
    %time_idx is the index along uref the initial condition is at
    xsize=(npred+1)*nstates;
    usize=npred*ninputs;

    Lb=[];
    Ub=[];
    for j=1:ninputs
    Lb=[Lb;input_range(j,1)-U_ref(j,initial_idx:initial_idx+npred-1)];
    Ub=[Ub;input_range(j,2)-U_ref(j,initial_idx:initial_idx+npred-1)];
    end

    Lb=reshape(Lb,[usize,1]);
    Ub=reshape(Ub,[usize,1]);

    Lb=[-Inf(xsize,1);Lb];
    Ub=[Inf(xsize,1);Ub];

end

function dzdt=kinematic_bike(t,x,U,T)
%constants
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;


%generate input functions
delta_f=U(1);
F_x=U(2);

%slip angle functions in degrees
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));

%Nonlinear Tire Dynamics
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));

F_zf=b/(a+b)*m*g;
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;

F_zr=a/(a+b)*m*g;
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;

F_total=sqrt((Nw*F_x)^2+(F_yr^2));
F_max=0.7*m*g;

if F_total>F_max
    
    F_x=F_max/F_total*F_x;
  
    F_yr=F_max/F_total*F_yr;
end

%vehicle dynamics
dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
          (-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
          x(2)*sin(x(5))+x(4)*cos(x(5));...
          (F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
          x(6);...
          (F_yf*a*cos(delta_f)-F_yr*b)/Iz];
end

%% nonlinear opt functions
function [lb, ub] = bounds(start_idx, end_idx)
    load('TestTrack.mat');
    load('reftrack_info.mat');
    
    %[m,nPts] = size(TestTrack.cline);
%     numState = 6;
%     numInput = 2;
   % nsteps = StepsPerPoint * nPts;
    
    lb_u = [-0.5;-5000];
    ub_u = [0.5;5000];

    bound_X = [TestTrack.bl(1,1), TestTrack.bl(1,2), ... 
               TestTrack.br(1,1), TestTrack.br(1,2)];
    bound_Y = [TestTrack.bl(2,1), TestTrack.bl(2,2), ... 
               TestTrack.br(2,1), TestTrack.br(2,2)];
    phi_init = TestTrack.theta(1);
    
    %phi restricted to just [-pi pi]
    %lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
    %ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
    lb = []; ub = [];
    
    %hijacking this for loop to only consider one index for bounds    
    for i=start_idx:end_idx+1
        Y_ref_curxy = [Y_submission(i,1); Y_submission(i,3)];
        sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2;
        dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
        [minDist, indMin] = min(dist_to_cl);
        
        prev_idx = max(indMin-1, 1);
        next_idx = min(indMin, 246);
        
        bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ... 
                   TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
        bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ... 
                   TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
        %phi_init = TestTrack.theta(i);
        
        lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
        ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
        
        
        lb=[lb;lb_x];
        ub=[ub;ub_x];
        
    end

    for i=start_idx:end_idx
        ub=[ub;ub_u];
        lb=[lb;lb_u];
    end
end

function [J, dJ] = costfun(z)
    
    global nsteps
    global target_vec
    load('TestTrack.mat');
    
    targetPos = target_vec(1:2);
    targetTheta = target_vec(3);
    sumPos = [];
    sumInput = [];
    for i = 1:nsteps
        zIndx = 6*(i-1) + 1;
        sumPos(i) = (z(zIndx) - targetPos(1))^2 + (z(zIndx+2) - targetPos(2))^2 + (z(zIndx+4) - targetTheta)^2;
       
        if (i <= nsteps-1)
            uInd = 2*(i-1) + nsteps*6 - 1;
            sumInput(i) = z(uInd)^2 + z(uInd+1)^2;
        end
    end

    J = sum(sumPos) + sum(sumInput);
    dJ = transpose(z.*2);
    
    uStart = nsteps*6 - 1;
    for i = 1:6:nsteps*6
        zIndx = i;
        dJ(i) = (z(zIndx) - targetPos(1))*2;
        dJ(i+2) = (z(zIndx+2) - targetPos(2))*2; 
        dJ(i+4) = (z(zIndx+4) - targetTheta)*2;
    end
        

end

function [g, h, dg, dh] = nonlcon(z)

    %nsteps = (size(z,2)/8);
    global nsteps
    global Xobs
    curr_pos = [z(1); z(3)];
  
    Xobs_seen = senseObstacles(curr_pos, Xobs);
    centroids = [];
    for i = 1:size(Xobs_seen,2)
        centroids = [centroids; mean(Xobs_seen{1, i})];
    end
    
    dt = 0.01;
    g = []; dg = [];
    
    %radius size
    r = 3;

    for i = 1:nsteps
        zInd_x = 6*(i-1) + 1;
        zInd_y = 6*(i-1) + 3;
        curr_xy = [z(zInd_x), z(zInd_y)];
        
        %g based on if there's obstacles around
        %initialize to zero
        g_curr = 0;
        zeroRow = zeros(1,size(z,2));   
        dg(i,:) = zeroRow;
        if (~isempty(centroids))
            dist2Obst = [];
            for j = 1:size(Xobs_seen,2)
                dist = norm(curr_xy(1) - centroids(j,1)) + norm(curr_xy(2) - centroids(j,2));
                dist2Obst = [dist2Obst; dist];
            end
            
            %closest obstacle used for constraint
            [minDist, indMin] = min(dist2Obst);
            
            %g
            g_curr = r^2 - (curr_xy(1) - centroids(indMin, 1))^2 - (curr_xy(2) - centroids(indMin, 2))^2;
            
            %dg                    
            dg(i,zInd_x) = curr_xy(1)*(-2) - centroids(indMin, 1)*2;
            dg(i,zInd_y) = curr_xy(2)*(-2) - centroids(indMin, 2)*2;
        end
        g = [g; g_curr];            
    end
    dg = transpose(dg);
    
    h = z(1:6);
    for i = 2:nsteps
        zInd_x = 6*(i-1) + 1;
        zInd_x_prev = 6*(i-2) + 1;
        
        stateCurr = z(zInd_x:zInd_x+5);        
        statePrev = z(zInd_x_prev:zInd_x_prev+5);   
        
        %get previous input
        uInd_prev = 2*(i-1) + nsteps*6 - 1;
        uPrev = [z(uInd_prev), z(uInd_prev + 1)];   
        
        derPrev= dt*bike(statePrev, uPrev);
        
        currH = stateCurr - statePrev - derPrev;
        h(6*(i-1)+1) = currH(1); 
        h(6*(i-1)+2) = currH(2);
        h(6*(i-1)+3) = currH(3);         
        h(6*(i-1)+4) = currH(4); 
        h(6*(i-1)+5) = currH(5);
        h(6*(i-1)+6) = currH(6); 
    end
    
    dh = zeros(size(z,2), nsteps*6);
    dh(1:6, 1:6) = eye(6);
    for i = 1:nsteps-1
        uInd = 2*(i-1) + nsteps*6 + 1;
        uCurr = [z(uInd), z(uInd + 1)];  
        zInd_x = 6*(i-1) + 1;
        stateCurr = z(zInd_x:zInd_x+5);
        [A, B] = bikeLinearize(stateCurr, uCurr);
        
        dh(6*i+1:6*i+6, 6*i+1:6*i+6) = eye(6);
        dh(6*i-5:6*i, 6*i+1:6*i+6) = -eye(6) - dt*A;
        dh(nsteps*6+2*i-1:nsteps*6+2*i, 6*i+1:6*i+6) = -dt*transpose(B);
    end
        
end

%% linearization functions

function dzdt=bike(x,U)
%constants
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;


%generate input functions
delta_f= U(1);
F_x= U(2);

%slip angle functions in degrees
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));

%Nonlinear Tire Dynamics
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));

F_zf=b/(a+b)*m*g;
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;

F_zr=a/(a+b)*m*g;
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;

F_total=sqrt((Nw*F_x)^2+(F_yr^2));
F_max=0.7*m*g;

if F_total>F_max
    
    F_x=F_max/F_total*F_x;
  
    F_yr=F_max/F_total*F_yr;
end

%vehicle dynamics
dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
          (-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
          x(2)*sin(x(5))+x(4)*cos(x(5));...
          (F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
          x(6);...
          (F_yf*a*cos(delta_f)-F_yr*b)/Iz];
end

function [A, B] =bikeLinearize(x,U)
%constants
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;


%generate input functions
delta_f= U(1);
F_x= U(2);

%slip angle functions in degrees
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));

%Nonlinear Tire Dynamics
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));

F_zf=b/(a+b)*m*g;
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;

F_zr=a/(a+b)*m*g;
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;

F_total=sqrt((Nw*F_x)^2+(F_yr^2));
F_max=0.7*m*g;

if F_total>F_max
    
    F_x=F_max/F_total*F_x;
  
    F_yr=F_max/F_total*F_yr;
end



%we are just going to use cornering stiffness to make linear so this derivative
%easier, the vehicle parameter's are close enough to problem 1 hw 2
B=10;
C=1.3;
D=1;
Ca_r= F_zr*B*C*D;
Ca_f= F_zf*B*C*D;

A = [0, cos(x(5)), 0, -sin(x(5)), x(2)*sin(x(5))-x(4)*cos(x(5)), 0;
         0, (-1/m)*Ca_f*x(2)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1;
         0, sin(x(5)), 0, cos(x(5)), -x(4)*sin(x(5))+x(2)*cos(x(5)), 0;
         0, (1/m)*(-Ca_f*x(2)^-2 - Ca_r*x(2)^-2) - 1, 0, Ca_r/m*(-1/x(2)) + Ca_f/m*(-1/x(2)), 0, Ca_r/m*(b/x(2)) + Ca_f/m*(-a/x(2)) - x(2);
         0, 0, 0, 0, 0, 1
         0, (1/Iz)*(-Ca_f*a*x(2)^-2 - b*Ca_r*x(2)^-2), 0, -b*Ca_r/Iz*(-1/x(2)) + a*Ca_f/Iz*(-1/x(2)), 0, -b*Ca_r/Iz*(b/x(2)) + a*Ca_f/Iz*(-a/x(2))];
     
B = [0, -Ca_f/(x(2)*m), 0, Ca_f/m, 0, a*Ca_f/Iz; 
     0, Nw/m, 0, 0, 0, 0]';
end