% Modello l'estremo che divide le sezioni di due camere
% come una massa con attrito. Metto la massa sennò le
% forze generate dalle 2 camere produrrebbero una accelerazione infinita;
% metto l'attrito sennò il sistema oscillerebbe all'infinito.

% Le camere producono su tale estremo una forza che è sostanzialmente quella
% di una molla estesa tra i 2 estremi della sezione. Mettendo tutte le
% costanti elastiche uguali i punti si distribuiscono in modo uniforme
% con distanza reciproca Ltot/N. Questo dunque è corretto se le velocità
% delle camere sono uguali. Se le velocità sono diverse invece dovrò
% ritoccare le k: una k più forte produrrà una sezione d'equilibrio più
% stretta, che va attribuita a una camera più lenta. Sarà dunque da
% ricavare la relazione tra v_i e k_i con v_i diverse. Non so se dipenderà
% anche dal numero totale di camere. Bisognerebbe vedere di ricavare la
% tensione sulla catena di molle e vedere com'è la dipendenza dei pti di
% equilibriodalle ki, ovviamente ammettendo che la soluzione sia
% realizzabile.

% Il caso subottimo viene modellato con una saturazione sul valore
% dell'ascissa degli estremi. In questo modo "automaticamente" il sistema
% converge a una soluzione subottima nelle 2 parti, e fa andare al bordo
% più vicino alla divisione ottima (fa saturare) l'estremo problematico.

% estremi fisici del percorso totale

Linf=0;
Lsup=5;
T=120;
Ts=0.05;

p_com=0.5; % probabilita' di comunicazione agli estremi

vmax=[0.18 0.23 0.15 0.25 0.16]; % m/s

fovi=[0.2 0.2 0.2 0.2 0.2];

vimax=vmax*Ts; % distanza da coprire in un istante di campionamento

t=(Ts:Ts:T)';

tcom=zeros(T/Ts,4);

for i=1:T/Ts
    for n=1:4
        num = rand(1);
        if num<p_com, tcom(i,n)=1; end    
    end
end 

tcom=[t tcom];

coeff_fase=0.7; % 0.5 ok, convergono abbastanza alla fase in 3-4 periodi
                % Impostare a zero per disattivare messa in fase
                
fase_mode=-1; % 1=in fase, -1=opp fase.

FdT_chiamata=tf([0.05 0],[1 -0.95],Ts); % gain statico 1
% filtro la velocità in uscita da kalman
% per calcolare e passare all'altra camera il
% tempo di arrivo dell'evento. Bilanciare il gain statico e la posizione
% del polo reale, che deve comunque essere vicino a uno.

% varianza rumore sul calcolo dell'estremo nelle camere. Permette di vedere
% il modo in cui gli estremi si riportano al consenso se la modellizzazione
% dell'estremo tra 2 camere dovesse per qualche motivo andare a differenziarsi.
noise_estremo=0%.0005;

% differenza tollerata tra le 2 versioni degli estremi prima di resettare
% l'integratore
tolleranza_estremo=0.001;

m=10;
% kg. La massa ottima e' 0.1kg con Ts=0.025. Provo a aumentarla x rallentare la
% convergenza e vedere influenza andamento bordi sul zig-zag della
% telecamera

ki=1./(vmax);   % N/m (Delta-l in metri)
psi=1; % damping factor. Ottimo=2. Sottosmorzo x vedere effetto oscillazioni
% psi = 2/radice di 2 dà massima velocità di salita con oscillaz
% psi = 2 dà massima velocità senza oscill

% aumento la forza tra gli estremi quando faccio tracking. Calcolo la forza
% media che mi aspetto interessare gli estremi, e sommo un offset pari al
% quadruplo di tale forza quando faccio tracking
Fopt=(Lsup-Linf)/sum(vmax);
bias_estr_track=4*Fopt;


for i=1:numel(ki)-1
    kiestr(i)=ki(i)+ki(i+1)
end
% l'estremo ha due molle attaccate, quindi considero una kiestr equivalente
% pari alla somma delle due costanti elastiche. Questa kiestr equivalente
% mi serve solo per calcolare il coefficiente di attrito ni ottimo per
% l'estremo

ni=psi*2*sqrt(kiestr*m);  % ATTENZIONE, MODIFICARE psi NON QUESTO 
% coeff di attrito viscoso Ns/m, tale da assicurare
% damping factor=psi per il singolo oscillatore armonico
% smorzato
                
% Vincoli superiori e inferiori in coordinate assolute alle variazioni
% degli estremi aestr delle sezioni. Unico requisito: non dare intersezione
% tra range fisico es camere 1 e 3, ovvero al più sovrapp tra due cam
% adiacenti. Altrimenti applicando F aggiuntiva per stringere il range
% durante il tracking potrebbe succedere la situazione assurda che
% l'estremo inf della camera che fa il tracking va oltre l'estremo ottimo 
% sup e viceversa (la molla con una forza grande e costante può 'rovesciarsi').
% Andrebbe migliorato imponendo l'estr sup con vincolo di
% saturazione inferiore l'estr inf corrente e viceversa...

vincolo_inf=[0.8   1.8    2.8   3.8];
vincolo_sup=[1.2   2.2    3.2   4.2];

x0cam=[0.7 1.5 2.2 3.1 4.9] % posizione iniziali telecamere

% Generazione evento
velMin=min(vmax); %indipendente dal Ts

% EVENTO OFF
% statoEvento = ones(2,T/Ts+1)*(-1);
% misureEvento = ones(1,T/Ts)*(-1);

% EVENTO BLOCCA CAM2 a 1.35
%statoEvento = [ [-1;0]*ones(1,T/(2*Ts))  [1.35;0]*ones(1,T/(2*Ts)+1)];
%misureEvento = [ ones(1,T/(2*Ts))*[-1]  ones(1,T/(2*Ts))*[1.35]];

% EVENTO ON
[statoEvento, misureEvento] = Evento(Lsup-Linf,T,Ts,velMin);
plot(misureEvento)


% per simout_nostroplot
limiteinf = [Linf vincolo_inf];
limitesup = [Lsup vincolo_sup];
limiti = [limiteinf limitesup];

% per 3D
rate=2;
range_sup=[vincolo_sup Lsup];
range_inf=[Linf vincolo_inf];
range_size=range_sup-range_inf;
centri=(range_sup+range_inf)/2;
centri=centri*10;