clc
clear
close all

load("workspace.mat")
ZFS = 200;
ns = 9;

TR=0; %[Nm] coppia resistente
omega_WG=  31.7; % [rad/s] , 

t_step= 5e-7; %[s]
n_step= 4000; 

t_sim=n_step*t_step;  %[s]




X_E = Simulink.Signal; X_E.Dimensions = [200 1];
Y_E = Simulink.Signal; Y_E.Dimensions = [200 1];
Q   = Simulink.Signal; Q.Dimensions   = [9 200];

M_b= ones(nb,1)*0.1; %massa di una sfera
I_b= ones(nb,1)*63.04; %inerzia di una sfera kg∙mm2 

I_nb=eye(nb);

%parametri per attrito
v_s=100;
v_d=1000;
mu_s=0.1;
mu_d=0.02; 


m_j= 0.1*ones(nb,1)*1/nb; % massa di un settore di ralla esterna
c_bearing= ones(nb,1)*1e-2; % [N*s/mm] smorzamento tra settori

c_WG_FS=1e-2; %smorzamento tra FS e ralla esterna

m_tooth= 1e-3; %massa di un dente
c_tooth= ones(ZFS,1)*1e-2; % [N/(m/s)]
c_i=0.01; %smorzamento interno denti

%% Parametri ingranamento

b = params.beta;
K = params.K_FS_CS;
m = params.m; h_a = params.h_a; h_f = params.h_f;
rho_a = params.rho_a; rho_f = params.rho_f;
gamma = params.gamma; s_rim_FS = params.s_rim_FS;
s_rim_CS = params.s_rim_CS; N = params.N;
FS_tooth_corr = params.FS_tooth_corr_factor;

%% --- Costruzione profili locali (FS e CS) ---
x_a = 0.4976 + FS_tooth_corr;
y_a = 0.1461;
x_oa = -x_a; %centro arco AB
y_oa = h_f + s_rim_FS - y_a; %centro arco AB
arg1 = min(max((h_a + y_a) / rho_a, -1), 1);
alfa_a = asin(arg1);
l1 = rho_a * (alfa_a - gamma); %lunghezza arco AB
s1 = linspace(0, l1, N);
x1 = rho_a * cos(alfa_a - s1 / rho_a) + x_oa; %arco AB
y1 = rho_a * sin(alfa_a - s1 / rho_a) + y_oa;

epsilon = atan2(y1(end) - y_oa, x1(end) - x_oa); %angolo di unione archi
x_of = (rho_a + rho_f) * cos(epsilon) + x_oa; %centro arco BC
y_of = (rho_a + rho_f) * sin(epsilon) + y_oa;
x_f = x_of - pi * m / 2;
y_f = y_of - h_f - s_rim_FS;

arg2 = min(max((h_f + y_f) / rho_f, -1), 1);
alfa_f = asin(arg2);
l2 = l1 + rho_f * (alfa_f - gamma);
s2 = linspace(l1, l2, N);
x2 = x_of - rho_f * cos(gamma + (s2 - l1) / rho_f);
y2 = y_of - rho_f * sin(gamma + (s2 - l1) / rho_f);

x_AC = [x1 x2]; y_AC = [y1 y2];
x_tip = linspace(-x_AC(1), x_AC(1), N);
y_tip = y_AC(1) * ones(1, N);
x_tooth_FS = [-fliplr(x_AC), x_tip, x_AC];
y_tooth_FS= [fliplr(y_AC), y_tip, y_AC] - s_rim_FS;

%%
cf_FS =1e-2 ; % [N/(m/s)]
Kt_FS= 344; % [Nm/rad] 
psi_0=0;


zero_th= 180;

psi_0_CS = z_sol(1+2*nb + 2*ZFS) ; %202 punti da 0 a 2pi

N=20;
k_FS_CS=K_FS_CS; %*21500;
c_FS_CS=1e-2; %smorzamento tra FS e CS

J_CS= 1 ; %5.44e-3; % inerzia circular spline
c_CS= 1; 

K_tooth=K_tooth ;%*860000;
K_flex=K_flex*1;