%-------------------------------------------------------------------------%
%%----------------Hito 2: Simulación de tráfico--------------------------%%
%-------------------------------------------------------------------------%

% ---- Parametros constantes ---- %
qc = 3000;       % ----- flujo critico
vc = 120;        % ----- vel. maxima/crític
kc = 25;         % ----- dens. critica (qc/vc)
kmax = 100;      % ----- dens. máxima
kmax_via = 60;   % ----- dens. max en la via
L = 5;           % ----- longitud por celda
numCeldas = 5;   % ----- numero de celdas (No confundir con longitud)
At = 1/120;      % ----- incremento de tiempo (horas)
t = 7:At:10;     % ----- vector de tiempo
f = length(t);   % ----- longitud del vector t

% ---- Iniciar matrices ---- %
q0 = zeros(1,f);         % ----- flujo auxiliar
Q0 = zeros(1,f);         % ----- flujo auxiliar
k0 = zeros(1,f);         % ----- dens. auxiliar
n = zeros(numCeldas,f);  % ----- num vehiculos
k = zeros(numCeldas,f);  % ----- densidad vehiculos
q = zeros(numCeldas,f);  % ----- flujo de vehiculos saliente (real)
Q = zeros(numCeldas,f);  % ----- flujo de vehiculos (teorico)

%% --- Simulacion 1: sin semaforos --- %
for j = 1:f                   % Avance del tiempo
    for i = 1:numCeldas       % Avance en secciones de via
        k(i,j) = n(i, j)/L;   % Calculo densidad
        Q(i,j) = funcTrafico(k(i,j), kc, kmax, qc); % calculo flujo
        if i < 5   % flujo en cada celda
            q(i,j) = min(Q(i,j), (kmax_via*L - n(i+1,j)) / At); % tras imponer restricción de flujo por otras celdas, obtenemos el flujo real
        else
            q(i,j) = Q(i,j); % cel 5 no esta limitada por la cap. de otra celda, por lo que se simplifica a esto
        end

        % para la celda auxiliar (la celda "cero") usaremos variables independientes de las otras usadas, como sugiere la doc.
        if i == 1 % la celda ficticia alimenta a la 1a celda real, empezamos por esa celda
            qentrada(j) = 4000*exp(-(t(j) - 8) ^2);  % función de demanda conocida como e(t) (se comporta en función de un pulso gaussiano)
            if k0(j) > 0
                Q0(j) = qentrada(j) + k0(j)*L / At; % si esta celda ficticia acumula vehiculos, se incrementa el flujo para añadirlos al arco lo antes posible
            else
                Q0(j) = qentrada(j); % si es insuficiente, se almacena
            end

            q0(j) = min(Q0(j), (kmax_via*L - n(1,j)) / At); % se impone restriccion y se obtiene un flujo "real"
            k0(j+1) = k0(j) + (qentrada(j) - q0(j))*At / L; % actualizamos el valor de la densidad
            n(i,j+1) = n(i,j) + (q0(j) - q(i,j))*At; % finalmente se ejecuta la ecuación de la ev. del tráfico.
        else
            % ecuacion que rige la evolución del trafico, ejecutada al final del bloque (pero para el resto de celdas)
            n(i,j+1) = n(i,j) + (q(i-1,j) - q(i,j))*At;
        end
    end
end

nsum_nosem = sum(n(:,1:f), 1); % sacar cuantos vehiculos hay en total en cada celda, en func del tiempo
qarco_nosem = q(numCeldas, 1:f); % sacar flujo de salida del arco en func del tiempo

% reinicio de variables para ejecutar de nuevo la sim con el semaforo
q0 = zeros(1,f);
Q0 = zeros(1,f);
k0 = zeros(1,f);
n = zeros(numCeldas,f);
k = zeros(numCeldas,f);
q = zeros(numCeldas,f);
Q = zeros(numCeldas,f);

%% ------ Simulación con semáforo ------ %
for j = 1:f
    for i = 1:numCeldas
        k(i, j) = n(i, j)/L;
        Q(i,j) = funcTrafico(k(i, j), kc, kmax, qc);

        % Se implementa funcSemaforo. El semaforo se coloca en la ultima region. Impone nuevas restricciones en la ultima celda
        if i < 5
            q(i,j) = min(Q(i,j), (kmax_via*L - n(i+1, j)) / At);
        else
            q(i,j) = min(Q(i,j), funcSemaforo(t(j))); % en vez del caso por defecto se implementa la funcSemaforo que limita el flujo de la celda 5
        end

        % de nuevo, el caso especial de celdas ficticias
        if i == 1
            qentrada(j) = 4000*exp(-(t(j) - 8) ^2);
            if k0(j) > 0
                Q0(j) = qentrada(j) + k0(j)*L / At;
            else
                Q0(j) = qentrada(j);
            end
            q0(j) = min(Q0(j), (kmax_via*L - n(1,j)) / At);
            k0(j+1) = k0(j) + (qentrada(j) - q0(j))*At / L;
            n(i,j+1) = n(i,j) + (q0(j) - q(i,j))*At;
        else
            n(i,j+1) = n(i,j) + (q(i-1,j) - q(i,j))*At;
        end
    end
end

nsum_sem = sum(n(:,1:f), 1);
qarco_sem = q(numCeldas, 1:f);

%% ----- Plot de los datos ----- %
figure(1)
subplot(2,1,1)
plot(t, nsum_nosem, 'b');
title('Num vehiculos tot. en el arco en func. del tiempo');
ylabel('Vehículos/hora');
hold on
subplot(2,1,2)
plot(t, qarco_nosem, 'b');
title('Flujo de salida del arco en func. del tiempo');
ylabel('Vehículos/hora');
hold off
saveas(figure(1), 'figura1sinSemaforo.png');

figure(2)
subplot(2,1,1)
plot(t, nsum_nosem, 'b');
hold on;
plot(t, nsum_sem, 'r');
title('Num vehiculos tot. en el arco en func. del tiempo');
ylabel('Vehículos/hora');
% --- juntos
subplot(2,1,2)
plot(t, qarco_nosem, 'b');
hold on;
plot(t, qarco_sem, 'r');
title('Flujo de salida del arco en func. del tiempo');
ylabel('Vehículos/hora');
legend('Sin semáforo', 'Con semáforo');
saveas(figure(2), 'figura2comparaSemaforo.png');