%% SIMULADOR DE CAIXA DE VELOCIDADES
%% ########################################################################
% Definição dos estados: ##################################################
%##########################################################################
% EMBRAIAGEM:                                                             #
% clutch_state=1 -> Embraiagem completamente não actuada (carro           #
%                   desembraiado) [emb=0]                                 #
% clutch_state=2 -> Embraiagem completamente actuada (carro embraiado):   #
%                   [emb=1]                                               #
% clutch_state=3 -> Carro a embraiar (rampa de 0 para 1):                 #
%                   [emb=(1/time_clutch)*(t-t0)]                          #
% clutch_state=4 -> Carro a desembraiar (rampa de 1 para 0):              #
%                   [emb=1-(1/time_clutch)*(t-t0)]                        #
%##########################################################################
% CARRO:                                                                  #
% car_state=1 -> O carro está em movimento com velocidade superior a 10   #
%                [km/h]. Esse movimento é feito para a frente. Neste      #
%                estado a gestão de mudanças é a normal.                  #
% car_state=2 -> Carro parado, velocidade é menor que 10 [km/h]. É dada   #
%                ordem para embraiar, e a mudança passa a ponto morto.    #
% car_state=3 -> Arranque para velocidades positivas. O arranque é dado   #
%                se o utilizador premir demais a embraiagem com este no   #
%                estado 2.                                                #
% car_state=4 -> Arranque para velocidades negativas. Só é feita gestão   #
%                da marcha atrás, e só é activado se o utilizador premir  #
%                o botão da GUI para o efeito.                            #
%##########################################################################
%                                                                         #
% Explicação dos parâmetros:                                              #
%                                                                         #
% time_clutch -> O parâmetro time_clutch é o tempo máximo que a embraiagem#
%                demora a actuar. Por agora esse parâmetro será definido  #
%                como sendo 1 [s], no entanto mais tarde derá de ser um   #
%                valor medido experimentalmente;                          #
%                                                                         #
% t0 -> O parâmetro t0 corresponde ao valor absoluto do instante de tempo #
%       em que a embraiagem começa a ser actuada/desactuada;              #
%                                                                         #
% t -> A variavel independente do sistema, correspondente á passagem to   #
%      tempo;                                                             #
%                                                                         #
% emb -> Valor actual da embraiagem.                                      #
%-------------------------------------------------------------------------#
% MUDANÇAS:                                                               #
% gear_state=1 -> Estado de mudança metida: [gear=n]                      #
% gear_state=2 -> Estado de aumento de mudança (state=2):                 #
% gear_state=3 -> Estado de redução de mudança (state=3):                 #
%##########################################################################
%% PROGRAMA:
% Limpa a consola:
clc
% Limpa todas as variáveis:
clear all
% Fecha qualquer janela que possa estar aberta:
close all
% Mudança começa como sendo uma primeira:
curr_gear=1;
% Definição da posição inicial da mudança:
new_gear=curr_gear;
% Criação do objecto TCP/IP para comunicação:
TCP_OBJECT=tcpip('192.168.1.123',120,'Timeout',1,'Terminator',3);
% Abre-se a comunicação, depois de a fechar, por questões de segurança:
fclose(TCP_OBJECT);
fopen(TCP_OBJECT);
% Envia-se ordem para inserir a mudança inicial:
fprintf(TCP_OBJECT,['sg ',int2str(curr_gear)]);
% E faz-se um getline temporario á porta, para limpar o buffer:
j=fgetl(TCP_OBJECT);
% Corre o ficheiro que contém os parâmetros editaveis:
EDITABLE_PARAMETERS
% ### GERAÇÃO DA GUI:######################################################
% Geração da figura para a GUI:
h_fig=figure;
% Set á variavel de controlo da figura:
set(h_fig,'position',[500,600,600,400],'units','pixel','resize','off','DockControls','off','menubar','none');
% Função de geração da GUI:
[h_bar_tilt,h_bar_accel,h_bar_brake,h_text_angle,h_text_zero_angle,h_text_min_angle,h_text_max_angle,h_text_accel,h_text_brake,h_edit_tilt,h_edit_accel,h_edit_brake,h_button_override,subp_gear,subp_clutch,subp_speed,h_toggle_reverse]=build_gui(h_fig,max_ang);
% #########################################################################
% Sets ás variaveis de inicialização do programa:
% O botão de marcha atrás é desactivado:
reverse_state=0;
% Desfinição do estado do carro como estando a andar para a frente a mais de 10 [km/h]:
car_state=1;
% Definição do estado inicial da mudança como mudança metida:
gear_state=1;
% Definição da embraiagem em baixo:
clutch_pos=0;
% E a posição incial da embraiagem é também em baixo:
init_clutch_pos=clutch_pos;
% Definição do estado inicial da embraiagem:
clutch_state=1;
% A aceleração imposta inicialmente é nula.
global_acceleration=0;%    [km/h²]
% Definição do tempo inicial (momento zero):
t=0;
% Definição do tempo antigo:
old_t=t;
% Variavel de arranque I - verificação se o carro necessita de arrancar ou se não:
need_start=0;
% Variavel de arranque II - Verifica se alguém já carregou no acelerador depois da mudança ser metida:
need_start_2=0;
% Variável de inicialização do t0_clutch:
t0_clutch=0;
% Variavel auxiliar para a sequencia de mudanças:
run_up=0;
run_dn=0;
% Variavel auxiliar para o estado 2:
run_state_2=0;
% Variavel auxiliar para o estado 3:
run_state_3=0;
% Variavel auxiliar para o estado 4:
run_state_4=0;
% Variável auxiliar para fazer o set á mudança:
gear_set_aux=0;
% Variavel para guardar o numero da iteração:
iter_number=1;

is_set_DN=0;

[curr_rpm,v_min,next_v_min,v_max,next_v_max,rpm_min,next_rpm_min,rpm_max,next_rpm_max]=calculate_rpm(curr_speed,curr_gear,new_gear,rpm_vector,vel_vector,wheel_radius,gear_ratios,differential_ratio);
curr_gear_ratio=gear_ratios(curr_gear+2);
while(1)
    %curr_speed
    % Leitura do valor das barras:
    [val_tilt,val_accel,val_brake]=read_vbar_values(h_bar_tilt,h_bar_accel,h_bar_brake);
    % Calcula a relação de transmissão actual:
    if (curr_gear>0)&&(curr_gear<6)
        curr_gear_ratio=gear_ratios(curr_gear+2);
    elseif curr_gear==6
        curr_gear_ratio=gear_ratios(1);
    %elseif curr_gear==0
    %    curr_gear_ratio=gear_ratios(2);
    end
    % Calculo das três acelerações necessárias:
    [acc_gravity,acc_motor,acc_brake,acc_road_resistance,acc_drag]=calculate_accelerations(val_tilt,max_ang,g,val_accel,val_brake,brake_max_force,curr_speed,clutch_pos,car_mass,tire_pressure,Cd,Frontal_area,rho_air,rpms,torques_engine,curr_rpm,curr_gear_ratio,differential_ratio,transmission_efficiency,wheel_radius,curr_gear);
    % Calculo da aceleração global a impor:
    global_acceleration=acc_gravity+acc_brake+acc_motor+acc_road_resistance+acc_drag;
    % Calculo da velocidade do veículo:
    curr_speed=calculate_speed(global_acceleration,seconds_to_hours(t),seconds_to_hours(old_t),curr_speed);
    % Cálculo das rpm actuais do veículo, bem como os limites de rpm e velocidade da mudança actual e da próxima:
    [curr_rpm,v_min,next_v_min,v_max,next_v_max,rpm_min,next_rpm_min,rpm_max,next_rpm_max]=calculate_rpm(curr_speed,curr_gear,new_gear,rpm_vector,vel_vector,wheel_radius,gear_ratios,differential_ratio);
    % Se a velocidade descer abaixo dos 10 [km/h] ou subir acima dos -10 [km/h], e o carro não estiver em estado de arranque:
    if (curr_speed<10)&&(curr_speed>-10)&&(car_state~=3)&&(car_state~=4)
        % O carro passa ao estado 2 (parado):
        car_state=2;
    end
    % #####################################################################
    % # VERIFICAÇÃO DO ESTADO DA EMBRAIAGEM: ##############################
    % #####################################################################
    % Verifica se é pedido o estado 1 da embraiagem:
    if(clutch_state==1)
        % Coloca a embraiagem na posição 0:
        clutch_pos=0;
        % Verifica se é pedido o estado 2 da embraiagem:
    elseif(clutch_state==2)
        % Coloca a embraiagem na posição 1:
        clutch_pos=1;
        % Verifica se é pedido o estado 3 da embraiagem:
    elseif(clutch_state==3)
        % Calcula o estado da embraiagem:
        clutch_pos=(1/time_clutch)*(t-t0_clutch);
        % No caso da embraiagem "saturar" a 1:
        if(clutch_pos>1)
            % Faz set da embraiagem a 1:
            clutch_pos=1;
            % Modifica o estado para 2:
            clutch_state=2;
        end
        % Verifica se é pedido o estado 4 da embraiagem:
    elseif(clutch_state==4)
        % Calcula o estado da embraiagem:
        clutch_pos=init_clutch_pos-(1/time_clutch)*(t-t0_clutch);
        % No caso da embraiagem "saturar" a 0:
        if(clutch_pos<0)
            % Faz set da embraiagem a 0:
            clutch_pos=0;
            % Modifica o estado a 1:
            clutch_state=1;
        end
    end
    %----------------------------------------------------------------------
    % CARRO NO ESTADO 1: --------------------------------------------------
    %----------------------------------------------------------------------
    if car_state==1
        %disp('Estado 1')
        % Calcula-se a mudança necessária para a velocidade e aceleração actual:
        necessary_gear=necessary_gear_calculator_2(curr_speed,100*val_accel,curr_gear);
        if (curr_gear~=necessary_gear)&&(clutch_state~=3)&&(clutch_state~=4)
            if necessary_gear>curr_gear
                % Incrementa-se a mudança:
                new_gear=necessary_gear;
                % Envia um sinal para subida de mudança:
                gear_state=2;
            elseif necessary_gear<curr_gear
                % Incrementa-se a mudança:
                new_gear=necessary_gear;
                % Envia um sinal para subida de mudança:
                gear_state=3;
            end
        end
        % #################################################################
        % # VERIFICAÇÃO DO ESTADO DAS MUDANÇAS: ###########################
        % #################################################################
        % No caso de estar no estado 2 (AUMENTO):
        if(gear_state==2)
            % Se for a primeira vez que entra neste ciclo:
            if(run_up==0)
                % Guarda-se o tempo para embraiagem:
                t0_clutch=t;
                % Incrementa-se a primeira contagem do ciclo:
                run_up=1;
            end
            % Dá-se ordem de embraiar:
            clutch_state=3;
            % Caso a embraiagem chegue ao estado completamente embraiado:
            if(clutch_pos==1)
                % Altera-se o estado da embraiagem para carro embraiado:
                clutch_state=2;
                % Efectua-se a comunicação com o dispositivo:
                read_gear=communication(TCP_OBJECT,new_gear);
                % Faz-se a verificação do estado de mudança que vem da caixa:
                if (read_gear>-1)&&(read_gear<7)
                    % E se for válido guarda-se o estado:
                    curr_gear=read_gear;
                end
                % Se a mudança actual for igual ao valor da mudança pedida:
                if(curr_gear==new_gear)
                    % A caixa passa ao estado 1:
                    init_clutch_pos=1;
                    % A caixa passa ao estado 1:
                    gear_state=1;
                    % Dá-se a ordem para desembraiar:
                    clutch_state=4;
                    % Reseta-se o tempo inicial de embraiagem:
                    t0_clutch=t;
                    run_up=0;
                end
            end
            
            % No caso de estar no estado 3 (REDUÇÃO):
        elseif(gear_state==3)
            % Se for a primeira vez que entra neste ciclo:
            if(run_dn==0)
                % Garante-se que a poisção da embraiagem é nula (carro embraiado):
                %clutch_pos=0;
                % Guarda-se o tempo para embraiagem:
                t0_clutch=t;
                % Incrementa-se a primeira contagem do ciclo:
                run_dn=1;
            end
            % Dá-se ordem de embraiar:
            clutch_state=3;
            % Caso a embraiagem chegue ao estado completamente embraiado:
            if(clutch_pos==1)
                % Altera-se o estado da embraiagem para carro embraiado:
                clutch_state=2;
                % Efectua-se a comunicação com o dispositivo:
                read_gear=communication(TCP_OBJECT,new_gear);
                % Faz-se a verificação do estado de mudança que vem da caixa:
                if (read_gear>-1)&&(read_gear<7)
                    % E se for válido guarda-se o estado:
                    curr_gear=read_gear;
                end
                if(curr_gear==new_gear)
                    % A posição inicial da embraiagem é como carregada:
                    init_clutch_pos=1;
                    % A caixa passa ao estado 1:
                    gear_state=1;
                    % Dá-se ordem para desembraiar:
                    clutch_state=4;
                    % Reseta-se o tempo inicial de embraiagem:
                    t0_clutch=t;
                    run_dn=0;
                end
            end
            % No caso de não haver alteração ao estado da mudança:
        else
            % A mudança mantém-se:
            curr_gear=new_gear;
        end
    end
    %----------------------------------------------------------------------
    % CARRO NO ESTADO 2: --------------------------------------------------
    %----------------------------------------------------------------------
    if car_state==2
        %disp('Estado 2')
        if (clutch_pos~=1)
        % Se for a primeira vez que entra neste ciclo:
        if(run_state_2==0)
            % Garante-se que a poisção da embraiagem é nula (carro embraiado):
            clutch_pos=0;
            % Guarda-se o tempo para embraiagem:
            t0_clutch=t;
            % Incrementa-se a variavel auxiliar:
            run_state_2=1;
        end
        % Dá-se ordem de embraiar:
        clutch_state=3;
        end
        % Caso a embraiagem chegue ao estado completamente embraiado:
        if(clutch_pos==1)&&(gear_set_aux==0)
            % Altera-se o estado da embraiagem para carro embraiado:
            clutch_state=2;
            % A nova mudança é um ponto-morto:
            new_gear=0;
            
        end
        % Envia-se a caixa de velocidades para ponto morto:
        read_gear=communication(TCP_OBJECT,new_gear);
        % Faz-se a verificação do estado de mudança que vem da caixa:
        if (read_gear>-1)&&(read_gear<7)
            % E se for válido guarda-se o estado:
            curr_gear=read_gear;
        end
        % Se entretanto o utilizador acelerar mais de 20%:
        if (100*val_accel)>minimum_start_percent
            % É lido constantemente o valor do botão de marcha-atrás:
            reverse_state=read_reverse_button_state(h_toggle_reverse);
            % Se o botão não estiver carregado:
            if reverse_state==0
                % Se entrar nesta situação pela primeira vez:
                if gear_set_aux==0
                    % É metida uma primeira:
                    new_gear=1;
                    % E o comando é enviado para a caixa:
                    communication(TCP_OBJECT,new_gear);
                    % Muda-se o estado da variavel auxiliar:
                    gear_set_aux=1;
                end
                if curr_gear==1
                    % Muda-se o estado da variavel auxiliar:
                    gear_set_aux=0;
                    % Reseta-se a variavel auxiliar:
                    run_state_2=0;
                    % Altera-se o estado do carro para o 3:
                    car_state=3;
                end
            % Se o botão estiver carregado:
            elseif reverse_state==1
                % Se entrar nesta situação pela primeira vez:
                if gear_set_aux==0
                    % É metida uma marcha-atrás:
                    new_gear=6;
                    % E o comando é enviado para a caixa:
                    communication(TCP_OBJECT,new_gear);
                    % Muda-se o estado da variavel auxiliar:
                    gear_set_aux=1;
                end
                if curr_gear==6
                    % Muda-se o estado da variavel auxiliar:
                    gear_set_aux=0;
                    % Reseta-se a variavel auxiliar:
                    run_state_2=0;
                    % Altera-se o estado do carro para o 4:
                    car_state=4;
                end
            end
        end
    end
    %----------------------------------------------------------------------
    % CARRO NO ESTADO 3: --------------------------------------------------
    %----------------------------------------------------------------------
    if car_state==3
        %disp('Estado 3')
        % Se for a primeira vez que entra neste ciclo:
        if run_state_3==0
            % Indica que a embraiagem começa na posição 1:
            init_clutch_pos=1;
            % A caixa passa ao estado 1:
            gear_state=1;
            % Dá-se a ordem para desembraiar:
            clutch_state=4;
            % Reseta-se o tempo inicial de embraiagem:
            t0_clutch=t;
            % Muda-se o estado da variavel auxiliar:
            run_state_3=1;
        end
        % Velocidade proporcional (positivamente) ao estado da embraiagem:
        curr_speed=(1-clutch_pos)*13;
        % Se a velocidade atingir 13 [km/h]
        if curr_speed==13
            % Reseta-se o tempo inicial de embraiagem:
            t0_clutch=t;
            % Reseta-se a variavel auxiliar:
            run_state_3=0;
            % Muda-se para o estado 1:
            car_state=1;
        end
    end
    %----------------------------------------------------------------------
    % CARRO NO ESTADO 4: --------------------------------------------------
    %----------------------------------------------------------------------
    if car_state==4
        %disp('Estado 4')
        % Se for a primeira vez que entra neste ciclo:
        if run_state_4==0
            % Indica que a embraiagem começa na posição 1:
            init_clutch_pos=1;
            % A caixa passa ao estado 1:
            gear_state=1;
            % Dá-se a ordem para desembraiar:
            clutch_state=4;
            % Reseta-se o tempo inicial de embraiagem:
            t0_clutch=t;
            % Muda-se o estado da variavel auxiliar:
            run_state_4=1;
        end
        % Velocidade proporcional (negativamente) ao estado da embraiagem:
        curr_speed=(1-clutch_pos)*(-13);
        % Se a velocidade atingir -13 [km/h]
        if curr_speed==-13
            % Reseta-se a variavel auxiliar:
            run_state_4=0;
            % Muda-se para o estado 1:
            car_state=5;
        end
    end
    %----------------------------------------------------------------------
    % CARRO NO ESTADO 5: --------------------------------------------------
    %----------------------------------------------------------------------
    if car_state==5
        %disp('Estado5')
        
    end
    %######################################################################
    % Acumulação dos vectores para obter resultados:
    % Posição da embraiagem:
    GLOBAL_CLUTCH_POS(1,iter_number)=clutch_pos;
    GLOBAL_CLUTCH_POS(2,iter_number)=t;
    % Velocidade do carro:
    GLOBAL_CAR_SPEED(1,iter_number)=curr_speed;
    GLOBAL_CAR_SPEED(2,iter_number)=t;
    % Current gear:
    GLOBAL_GEAR(1,iter_number)=curr_gear;
    GLOBAL_GEAR(2,iter_number)=t;
    % Tilt bar Value:
    GLOBAL_TILT_VALUE(1,iter_number)=val_tilt;
    GLOBAL_TILT_VALUE(2,iter_number)=t;
    % Acceleration Value:
    GLOBAL_ACCELERATION_VALUE(1,iter_number)=val_accel;
    GLOBAL_ACCELERATION_VALUE(2,iter_number)=t;
    % Brake Value:
    GLOBAL_BRAKE_VALUE(1,iter_number)=val_brake;
    GLOBAL_BRAKE_VALUE(2,iter_number)=t;
    
    %######################################################################
    % Faz-se um update aos plots da interface:
    
    
    
    
    update_plots(subp_speed,curr_speed,subp_clutch,clutch_pos,subp_gear,curr_gear,t);
    % Gravação do tempo antigo:
    old_t=t;
    % Incremento temporal:
    t=t+0.1;
    % Incremento do numero da iteração:
    iter_number=iter_number+1;
    
    % Pausa para actualização da GUI:
    pause(0.05)
end