%% prog diferencias centrales

%este programa resuelve la EDO de segundo orden con condiciones inciales
%usando diferencias finitas

clearvars;
close all;
clc;

%% datos inciales

M = [1,0,0;0,2,0;0,0,3];                    %matriz de masa
K = [40,-20,0;-20,40,-20;0,-20,20];         %matriz de rigidez
C = [10,-5,0;-5,10,-5;0,-5,5];              %matriz de amortiguamiento
      %amortiguador viscoso

%% tiempo

dt = 0.01;          %intervalo de muestreo
tini = 0;           %tiempo incial 
tfin = 100;         %tiempo final
t = tini:dt:tfin;   %vector de tiempo
N = length(t);      %numero de muestras total

%% condiciones inciales

x0 = [0;0;0];     %desplazamiento incial
v0 = [0;0;0];     %velocidad incial

%% fuerza externa

%componentes
f1(1:N) = 0;            %incializacion de fuerza
f2(1:N) = 0;            %incializacion de fuerza
f3(1:N) = 0;            %incializacion de fuerza

tf1 = 10;             	%tiempo1
tf2 = 30;             	%tiempo2
nf1 = round(tf1/dt);    %muestra1
nf2 = round(tf2/dt);    %muestra2

f1(nf1:nf2) = 50;

f = [f1;f2;f3];

%% desplazamiento

%inicializamos el desplazamiento
X = zeros(3,N);

%paso 1 determinar la aceleracion

f0 = f(:,1);
a0 = M\(f0 - x0 - v0);

%paso 2 determinar el desplazamiento x1 ubicado en la segunda columna de X
X(:,2) = x0 + v0*dt + (1/2)*a0*dt^2;

%paso 3 determinamos el resultado mediante recurrencia
X(:,1) = x0;


C1 = M/(dt^2) + C/(2*dt);
C2 = K - (2*M)/(dt^2);
C3 = M/(dt^2) - C/(2*dt);

for n = 2:N-1
    X(:,n+1) = C1\( f(:,n) - C2*X(:,n) - C3*X(:,n-1) );
end;

%separamos la solucion por facilidad
x1(1:N) = 0;   %incializacion de desplazamiento
x2(1:N) = 0;   %incializacion de desplazamiento
x3(1:N) = 0;   %incializacion de desplazamiento

for n = 1:N
    x1(n) = X(1,n);
    x2(n) = X(2,n);
    x3(n) = X(3,n);
end; 

%% velocidad

v1(1:N) = 0;   %incializacion de la velocidad
v2(1:N) = 0;   %incializacion de la velocidad
v3(1:N) = 0;   %incializacion de la velocidad

for n = 2:N-1
    v1(n) = ( x1(n+1) - x1(n-1) )/(2*dt);
    v2(n) = ( x2(n+1) - x2(n-1) )/(2*dt);
    v3(n) = ( x3(n+1) - x3(n-1) )/(2*dt);
end;

V = [v1;v2;v3];

%% resultados

figure(1)
plot(t,f1,'b',t,f2,'k',t,f2,'r');
title('fuerza');
xlabel('tiempo t(s)');
ylabel('fuerza f(t) (N)');
legend('f_1(t)','xf_2(t)','f_3(t)');
axis([tini, tfin, -1.1*max(abs(f1)), 1.1*max(abs(f1))]);
grid on;
box on;

figure(2)
plot(t,x1,'b',t,x2,'k',t,x3,'r');
title('desplazamiento diferencias centrales');
xlabel('tiempo t(s)');
ylabel('desplazamiento x(t) (m)');
legend('x_1(t)','x_2(t)','x_3(t)');
axis([tini, tfin, -1.1*max(max(abs(X))), 1.1*max(max(abs(X)))]);
grid on;
box on;

figure(3)
plot(t,v1,'b',t,v2,'k',t,v3,'r');
title('velocidad diferencias centrales');
xlabel('tiempo t(s)');
ylabel('velocidad v(t) (m/s)');
legend('v_1(t)','v_2(t)','v_3(t)');
axis([tini, tfin, -1.1*max(max(abs(V))), 1.1*max(max(abs(V)))]);
grid on;
box on;


figure(4)
plot(x1,v1,'b',x2,v2,'k',x3,v3,'r');
title('diagrama de fase diferencias centrales');
xlabel('desplazamiento x(t) (m)');
ylabel('velocidad v(t) (m/s)');
legend('x_1(t) vs v_1(t)','x_2(t) vs v_2(t)','x_3(t) vs v_3(t)');
axis([-1.1*max(max(abs(X))), 1.1*max(max(abs(X))), -1.1*max(max(abs(V))), 1.1*max(max(abs(V)))]);
grid on;
box on;