function MAIN_control_Xplane
addpath('../lib/xplane');
clc; clear all; close all;

run Configure_XPLANE;

%##########################################################################
% CONTROL LOOP
%##########################################################################
PERIOD=0.2;
try
    PHASE=1;
    global stop;
    stop=false;
    
    while (~stop)
        t0=tic;
        % Read data from Xplane
        data=dref_readXPLANE();
        
        switch PHASE
            case 1
                [actions,PHASE]=rollout(data);
            case 2
                [actions,PHASE]=rotate(data);
            case 3
                [actions,PHASE]=climb(data);
            case 4
                [actions,PHASE]=cruise(data);
        end
        
        dref_displayXPLANE(data);
        fprintf('\n \nPHASE: %g\n',PHASE);
        fprintf('Ailerons: %g, Rudder: %g, Elevators: %g, Throttle: %g, Brake: %g\n',actions(AILERONS),actions(RUDDER),actions(ELEVATORS),actions(THROTTLE),actions(PARKBRAKE));
        
        % Write parameters into Xplane
        dref_writeXPLANE(ip,port_Xplane,dataref_w,actions);
        
        % Make it periodic with period 0.2
        delay=PERIOD-toc(t0);
        pause(delay);
        fprintf('delay: %g\n',delay);
    end
    
catch err
    rethrow(err);
end
end
