...

function [system, objective, init, designC, stateC, objComp] = ...
    qcar_new_objective_open()
% Quarter-car new objective with design and state constraints
param.ms = 325;               % 1/4 sprung mass (kg)
param.mus = 65;               % 1/4 unsprung mass (kg)
param.kus = 232.5e3;          % tire stiffness (N/m)

param.ct = 0 ;          
 
rgrade = 25;            % ramp grade for rattle space test (percent)
v1 = 10;                % vehicle velocity for rattle space test (m/s)
param.v = v1;
param.rgrade = rgrade;
% Design variables
d = 0.01;            % d: spring wire diameter 
D = 0.129;           % D: spring helix diameter
p = 0.106;           % p: spring pitch
Na = 3.57;           % Na: number of active spring coils 
Do = 0.006;          % Do: orifice diameter (m)
Dp = 0.035;          % Dp: piston diameter (m)
Ds = 0.17;           % Ds: damper stroke (m)

spring_vars = [d;D;p;Na];
damper_vars = [Do; Dp; Ds];
xd = [spring_vars; damper_vars];

system = System;
system.parameters =param;
system.deriv  = @(t,x,u,xd, input) f(t,x,u,xd, param, input);
system.jacobian = @(t,x,u,xd, input) jacobian(t,xd, param);
system.jacobian_d = @(t,x,u,xd) jacobian_d(t,x, xd, param);

objective = Objective;
objective.fun = @(x_d,xin, t,input) obj_f(xin, x_d, t, param, input);
objective.gradient = @(x_d,xin,t, input) obj_f_gradient(xin, x_d, t, param);
objective.hessian =  @(x_d,xin,t) obj_f_hessian(xin, x_d, t, param);
objective.gradient_d = @(x_d,xin,t,input)obj_f_design_gradient(xin,x_d,t,param,input);

objComp = @(x_d,xin, t,input) obj_components(xin, x_d, t, param, input);

init = InitCondition;

init.fun = @(xd_)X0(xd_, param);
init.jacobian = @(xd_) X0Jacobian(xd_, param);

designC = DesignConstraint;
designC.fun = @(xd_) designConstraint(xd_, param);
designC.jacobian = @(xd_) designConJacobian(xd_, param);

stateC = StateConstraint;
stateC(2) = StateConstraint;
stateC(1).fun = @(xd_, x_) rampStateCon(xd_, x_, param);
stateC(1).jacobian = @(xd_, x_) rampConJacobian(xd_, x_, param);
stateC(2).fun = @(xd_, x_) roughStateCon(xd_, x_, param);
stateC(2).jacobian = @(xd_, x_) roughConJacobian(xd_, x_, param);
end

function dx = f(t, x, u, xd, param, input)
J = jacobian(t, xd, param);
A = J(1:4,1:4);
B = J(1:4, 5);
b_ramp = [-1; param.ct/param.mus; 0; 0];
if ~isempty(input)   
    dx= A* x + B * u + b_ramp * input(t); 
else
    dx = A*x + B*u ;
end
end


function J = jacobian(~, xd, param)
ks = spring_constant(xd);
cs = damper_constant(xd);
A = [ 0 1 0 0;
    -param.kus/param.mus -cs/param.mus ks/param.mus cs/param.mus;
    0 -1 0 1;
    0 cs/param.ms -ks/param.ms -cs/param.ms];
B = [0 -1/param.mus 0 1/param.ms]'; 
J = [A, B];
end

function ks = spring_constant(xd)
% Calculate the spring constant ks from the design variables
d = xd(1);
D = xd(2);
Na = xd(4);
G = 77.2e+9; 
C = D/d;
ks = d^4 * G/ (8* D^3*Na*(1+1/2/C^2));
end

function cs = damper_constant(xd)
% Calculate the damper constant cs from the design variables
Do = xd(5);             % orifice diameter
Dp = xd(6);             % piston diameter

rho1 = 850;             % density of oil (kg/m^3)
kv = 7500;              % damper valve spring rate (N/m)
Cd = 0.7;               % valve discharge coefficient
Afa = 0.1;              % damper valve area factor 
Pallow = 4.75e6;        % maximum allowed damper pressure (Pa)
xi = 0.9;               % damper valve circumference factor

Ao = pi*Do^2/4;         % valve pressure surface area (M^2)
xm = Ao*Pallow/kv;      % lift @ Pallow (m)
C2 = xi*Afa*sqrt(xm);   % damper valve parameter (m^0.5)

cs = Dp^4*sqrt(kv* pi*rho1/2)/(8*Cd*Do^2*C2); %
end

function J = jacobian_d(t, x, xd, param)
J = perturb_x(@(xd)f(t,x,0, xd, param, []), xd);
end

function val = obj_f(xu, x_d, t, param, input)
xu_frame = xu;
val = 0;
R = Weight();
xu_frame_zs = zeros(6,length(t));

for i=1:(size(xu_frame,2)-1)
    dx = f(t(i),xu_frame(1:4,i), xu_frame(5,i), x_d, param, input);
    xu_frame_zs(:,i) = [xu_frame(:,i); dx(4)];
    val = val+ (t(i+1)-t(i))*xu_frame_zs(:,i)'*R*xu_frame_zs(:,i);
end
end

% For plotting the components of the objective functions
function wt = obj_components(xu, x_d, t, param, input)
xu_frame = xu;
R = Weight_components();
wt = zeros(length(R), length(t)-1);
xu_frame_zs = zeros(6,length(t));

for i=1:(size(xu_frame,2)-1)
    dx = f(t(i),xu_frame(1:4,i), xu_frame(5,i), x_d, param, input);
    xu_frame_zs(:,i) = [xu_frame(:,i); dx(4)];
    for j = 1:length(R)
        wt(j,i) = xu_frame_zs(:,i)'*R{j}*xu_frame_zs(:,i);
    end
end
end

function J = obj_f_gradient(xu_frame, x_d, t, param)
nt = numel(xu_frame)/5;
R_gradient = Weight_gradient(x_d,param);
J = zeros(numel(xu_frame),1);
for i=1:(nt-1)
    J((i-1)*5+1:i*5) = 2*(t(i+1)-t(i))*R_gradient*xu_frame(:,i);
end
end

function H = obj_f_hessian(xin, x_d, t, param)
nt =  numel(xin)/5;
R_gradient = Weight_gradient(x_d,param);
H = zeros(numel(xin), numel(xin));
for i=1:nt
    for j=1:5
        for k =1:5
        H((i-1)*5+j,(i-1)*5+k) = R_gradient(j,k);
        end
    end
end

end

function x0 = X0(xd, param)
x0 = [0;0;0;0];
end

function J = X0Jacobian(xd, param)
x0 = X0(xd, param);
J = zeros(size(x0,1), size(xd,1));
end


function R = Weight()
r1 = 1e+5;
r2 = 0.5;
q = 1e-5;
R = diag([r1, 0, 0, 0, q, r2]);
end


function R = Weight_components()
r1 = 1e+5;
r2 = 0.5;
q = 1e-5;
R{1} = diag([r1, 0, 0, 0, 0,0]);
R{2} = diag([0 0 0 0 q 0]);
R{3} = diag([0 0 0 0 0 r2]);
end

function R_gradient = Weight_gradient(xd,param)
r1 = 1e+5;
r2 = 0.5;
q = 1e-5;
ks = spring_constant(xd);
cs = damper_constant(xd);
R_gradient = [
    r1 0 0 0 0;...
    0 r2*(cs/param.ms)^2 -r2*(cs/param.ms)*(ks/param.ms) -r2*(cs/param.ms)^2 r2*cs/(param.ms^2);...
    0 -r2*(cs/param.ms)*(ks/param.ms) r2*(ks/param.ms)^2 r2*(cs/param.ms)*(ks/param.ms) -r2*ks/(param.ms^2);...
    0 -r2*(cs/param.ms)^2 r2*(cs/param.ms)*(ks/param.ms) r2*(cs/param.ms)^2 -r2*cs/(param.ms^2);...
    0 r2*cs/(param.ms^2) -r2*ks/(param.ms^2) -r2*cs/(param.ms^2) q+r2/(param.ms^2)];
end

function g = designConstraint(xd, param)
d = xd(1);      % d: spring wire diameter 
D = xd(2);      % D: spring helix diameter
p = xd(3);      % p: spring pitch
Na = xd(4);     % Na: number of active spring coils 
Do = xd(5);     % orifice diameter
Dp = xd(6);     % piston diameter
Ds = xd(7);     % damper stroke 

d_bar = 1000*d;
G = 77.2e+9; 
L0max = 0.40;
DoMax = 0.25;   
nd = 1.2;               % design factor (safety factor)
A = 1974;
m = 0.108;
Sut = A*1e6/(d_bar^m);
Ssy = 0.65*Sut; 
dsc = 0.009;            % damper-spring clearance (m)
dwt = 0.002;            % damper wall thickness (m)
LB = 0.02;              % bump stop length (m)
ld1 = 0.02;             % axial length required for upper damper mounting/valving
ld2 = 0.04;             % axial length required for lower damper mounting/valving
ld3 = 0.02;             % axial length required for lower damper casing extension

C = D/d;
L0 = p*Na + 2*d;
Ls = d*(Na + 1.75 - 1);
ks = d^4 * G/ (8* D^3*Na*(1+1/2/C^2));
Fs = ks*(L0 - Ls);
KB = (4*C+2)/(4*C-3);   % Bergstrausser factor
tau_s = KB*8*Fs*D/(pi*d^3); 
delta_g = param.ms*9.81/ks; 
Se2 = 0.24*Sut/nd;      % shear endurance strength (Stoicescu)

rho1 = 850;             % density of oil
kv = 7500;              % damper valve spring rate(N/m)
Afa = 0.1;              % damper valve area factor
xi = 0.9;               % damper valve circumfefence factor
Cd = 0.7;               % valve discharge coefficient
Pallow = 4.75e6;        % maxium allowed damper pressure(pa)
x3d_allow = 5.0;        % maxium damper velocity(m/s)
xv_allow = 0.03;        % maxium dampter valve lift(m)

g = zeros(9,1);
g(1) = 4 - C;           % spring manufacturing constraint
g(2) = C - 12;          % spring manufacturing constraint
g(3) = L0 - 5.26*D;     % spring stability/buckling constraint for squared ground ends
g(4) = L0 - L0max;      % spring packaging constraint
g(5) = d + D - DoMax;   % spring packaging constraint (shouldn't be active: constrained by g3)
g(6) = d - D + Dp + 2*(dsc+dwt);       % spring-damper interference constraint
% suspension stop constraint (ensures don't hit stops during test bump).
g(7) = (tau_s*nd - Ssy)/Ssy;           % shear stress constraint (note scaling)
g(8) = L0 - Ls - Ds;                   % ensures adequate damper stroke
g(9) = 2*Ds + ld1 + ld2 - L0max;       % ensures enough space for damper
end

function dg = designConJacobian(xd, param)
dg = perturb_x(@(xd_) designConstraint(xd_, param), xd);
end

function g = rampStateCon(xd, x, param)
d = xd(1);      % d: spring wire diameter 
D = xd(2);      % D: spring helix diameter
p = xd(3);      % p: spring pitch
Na = xd(4);     % Na: number of active spring coils 
Do = xd(5);     % orifice diameter
Dp = xd(6);     % piston diameter

G = 77.2e+9; 
LB = 0.02;      % bump stop length (m)
C = D/d;
L0 = p*Na + 2*d;
Ls = d*(Na + 1.75 - 1);
ks = d^4 * G/ (8* D^3*Na*(1+1/2/C^2));
delta_g = param.ms*9.81/ks; 

rho1 = 850;             % density of oil
kv = 7500;              % damper valve spring rate(N/m)
Afa = 0.1;              % damper valve area factor
xi = 0.9;               % damper valve circumfefence factor
Cd = 0.7;               % valve discharge coefficient
Pallow = 4.75e6;        % maxium allowed damper pressure(pa)
x3d_allow = 5.0;        % maxium damper velocity(m/s)
xv_allow = 0.03;        % maxium dampter valve lift(m)

Ao = pi*Do^2/4;         % valve pressure surface area (M^2)
xm = Ao*Pallow/kv;      % lift @Pallow (m)
C2 = xi*Afa*sqrt(xm);   % damper valve parameter (m^0.5)
KvC2 = sqrt(kv)/C2;     % valve parameter: sqrt(kv)/C2 (n^0.5/m)
cs = Dp^4*KvC2*sqrt(pi*rho1/2)/(8*Cd*Do^2);   % damping ratio coefficient cs

g(1) = - L0 + Ls +LB + delta_g + x(3);
Pmax = 4*cs*(-x(2)+x(4))/(pi*Dp^2);   % maximum damper pressure
g(2) = (Pmax - Pallow)/Pallow;       % damper pressure constraint
g(3) = (-Pmax - Pallow)/Pallow;
g(4) = -x(2) + x(4) - x3d_allow;
g(5) = x(2) - x(4) - x3d_allow;
xvmax = Ao*Pmax/kv;
g(6) = (xvmax - xv_allow)/xv_allow; % damper valve lift constraint
g(7) = (-xvmax - xv_allow)/xv_allow;
end

function g = roughStateCon(xd, x, param)
d = xd(1);      % d: spring wire diameter 
D = xd(2);      % D: spring helix diameter
p = xd(3);      % p: spring pitch
Na = xd(4);     % Na: number of active spring coils 
Do = xd(5);     % orifice diameter
Dp = xd(6);     % piston diameter
nd = 1.2;       % design factor (safety factor)
A = 1974;
m = 0.108;
G = 77.2e+9; 
LB = 0.02;      % bump stop length (m)

C = D/d;
L0 = p*Na + 2*d;
Ls = d*(Na + 1.75 - 1);
ks = d^4 * G/ (8* D^3*Na*(1+1/2/C^2));
Fs = ks*(L0 - Ls);
KB = (4*C+2)/(4*C-3);   % Bergstrausser factor
tau_s = KB*8*Fs*D/(pi*d^3); 
delta_g = param.ms*9.81/ks; 
d_bar = 1000*d;
Sut = A*1e6/(d_bar^m);
Ssy = 0.65*Sut; 
Se2 = 0.24*Sut/nd;      % shear endurance strength (Stoicescu)

rho1 = 850;             % density of oil
kv = 7500;              % damper valve spring rate(N/m)
Afa = 0.1;              % damper valve area factor
xi = 0.9;               % damper valve circumfefence factor
Cd = 0.7;               % valve discharge coefficient
Pallow = 4.75e6;        % maxium allowed damper pressure(pa)
x3d_allow = 5.0;        % maxium damper velocity(m/s)
xv_allow = 0.03;        % maxium dampter valve lift(m)

Ao = pi*Do^2/4;         % valve pressure surface area (M^2)
xm = Ao*Pallow/kv;      % lift @Pallow (m)
C2 = xi*Afa*sqrt(xm);   % damper valve parameter (m^0.5)
KvC2 = sqrt(kv)/C2;     % valve parameter: sqrt(kv)/C2 (n^0.5/m)
cs = Dp^4*KvC2*sqrt(pi*rho1/2)/(8*Cd*Do^2);   % damping ratio coefficient cs
g = zeros(9,1);
g(1) = 0.15+1-(L0-Ls)/(delta_g+x(3)*1.1);
Fmax = ks * (x(3)+ delta_g);        % maxium spring axial force
Fmin = ks * (delta_g - x(3));       % minium spring axial force
Fm = (Fmax + Fmin)/2;               % mean spring axial force
Fa = (Fmax - Fmin)/2;               % spring axial force amplitude
tau_m = KB*8*Fm*D/(pi*d^3);
tau_a = KB*8*Fa*D/(pi*d^3);

g(2) = tau_a/Se2 + tau_m/Ssy -1;    % Soderberg fatigue criterion
g(3) = (tau_a*nd-241e+6)/241e+6;    % Zimmerli fatigue criterion

Pmax = 4*cs*(-x(2)+x(4))/(pi*Dp^2); % maximum damper pressure
g(4) = (Pmax - Pallow)/Pallow;      % damper pressure constraint
g(5) = (-Pmax - Pallow)/Pallow;
g(6) = -x(2) + x(4) - x3d_allow;
g(7) = x(2) - x(4) - x3d_allow;     % damper velocity constraint

xvmax = Ao*Pmax/kv;
g(8) = (xvmax - xv_allow)/xv_allow; % damper valve lift constraint
g(9) = (-xvmax - xv_allow)/xv_allow;
end


function [dg_d, dg] = rampConJacobian(xd, x, param)
dg_d = perturb_x(@(xd_) rampStateCon(xd_, x, param), xd);
dg = perturb_x(@(x_) rampStateCon(xd, x_, param), x);
end
function [dg_d, dg] = roughConJacobian(xd, x, param)
dg_d = perturb_x(@(xd_) roughStateCon(xd_, x, param), xd);
dg = perturb_x(@(x_) roughStateCon(xd, x_, param), x);
end
function J = obj_f_design_gradient(xu, x_d, t, param, input)
J = perturb_x(@(x_d) obj_f(xu, x_d, t, param,input), x_d)';
end