%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Initialize GenoM stuff and connect the ports %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% %%%%%%%%%%%% SETUP %%%%%%%%%%%%%%%%%%%%%%%
filename_imuCalibration = 'QR7_imu_calibration';
quad_num = '7';

switch flag_simulationReal
    case 1 % Simulation
        disp('Initialize genom components for simulation');
        host_base = 'localhost';
        host_remote = 'localhost';
    case 2 % Real experiment
        disp('Initialize genom components for real experiment');
        host_base = 'localhost';
        host_remote = 'fritillary-wifi';
end

openrobots_dir = getenv('ROBOTPKG_BASE');
% Load genomix client
client = genomix.client(host_remote);
client.rpath([openrobots_dir,'/lib/genom/pocolibs/plugins']);

%% %%%%%%%%%%%% MRSIM %%%%%%%%%%%%%%%%%%%%%%%
if flag_simulationReal == 1
    disp('* Initialize Mrsim'); 
    mrsim = client.load('mrsim');
    string = 'Connecting to mrsim: done';
    disp(string);
    mrsim.set_pty('/tmp/mrsim');
end

%% %%%%%%%%%%%% ROTORCRAFT %%%%%%%%%%%%%%%%%
disp('* Initialize Rotorcraft');
rotorcraft = client.load('rotorcraft');
pause(1);

switch flag_simulationReal
    case 1 % Simulation
        result_mk = rotorcraft.connect('/tmp/mrsim', 500000);
    case 2 % Real experiment
        result_mk = rotorcraft.connect('/dev/ttyUSB0', 500000);  
end
string = ['Connecting to rotorcraft: ',result_mk.status];
disp(string);

% calibration
if flag_simulationReal == 2 
    load(strcat(pwd, filesep, 'calibration', filesep, filename_imuCalibration));
    eval('calibration');
    rotorcraft.set_imu_calibration(calibration);
end

% log of files
rotorcraft.set_sensor_rate(1000,16,50,1); % IMU - mag - motor - battery

% connection to nhfc
result_nhfc = rotorcraft.connect_port('rotor_input' , 'nhfc/rotor_input');
string = ['Initializing connection of Nhfc: ',result_nhfc.status];
disp(string);

%% %%%%%%%%%%%%% OPTITRACK %%%%%%%%%%%%%%%%%%
if flag_simulationReal == 2
    disp('* Initialize Optitrack');
    opti = client.load('optitrack');
    pause(1);
    
    result = opti.connect('marey', '1510', '239.192.168.30', '1511');
    string = ['Connecting to MoCap: ', result.status];
    disp(string);
end

%% %%%%%%%%%%%%% POM %%%%%%%%%%%%%%%%%%%%%%%%
disp('* Initialize Pom');
pom = client.load('pom');
pause(1);

result_pom = pom.connect_port('measure/imu', 'rotorcraft/imu');
string = ['Initializing 1st connection of POM (imu): ', result_pom.status];
pom.add_measurement('imu');
disp(string);

switch flag_simulationReal
    case 1 % Simulation
        result = pom.connect_port('measure/gps', 'mrsim/gps');
        string = ['Initializing 2nd connection of POM (mrsim/gps): ',result.status];
        pom.add_measurement('gps');
        disp(string);
    case 2 % Real experiment    
        result = pom.connect_port('measure/mocap', strcat('optitrack/bodies/QR_',quad_num));
        string = ['Initializing 2nd connection of POM (optitrack): ',result.status];
        pom.add_measurement('mocap');
        disp(string);
end

%% %%%%%%%%%%%%% NHFC %%%%%%%%%%%%%%%%%%%%%%%
disp('* Initialize Nhfc');
nhfc = client.load('nhfc');
nhfc.set_mass(mR);
nhfc.set_geom(G_UAV,J_UAV);
nhfc.set_servo_gain(Kp_p(1,1) , Kp_p(3,3) , Kp_R(1,1) , Kp_R(3,3) , Kd_p(1,1) , Kd_p(3,3) , Kd_R(1,1) , Kd_R(3,3), 0, 0); % Kpxy, Kpz, Kqxy, Kqz, Kvxy, Kvz, Kwxy, Kwz,
nhfc.set_emerg(1, 0.03, 1, 0.03, 1); % descent acc, uncer pos (3*std), uncer orien, uncer vel, unc ang vel
result = nhfc.connect_port('state', 'pom/frame/robot');
string = ['Conncting to pom: ',result.status];
disp(string);
result = nhfc.connect_port('reference', 'maneuver/desired');
string = ['Conncting to maneuver: ',result.status];
disp(string);

%% %%%%%%%%%%%%% MANEUVER %%%%%%%%%%%%%%%%%%%%%%%
disp('* Initialize Maneuver');
maneuver = client.load('maneuver');
result = maneuver.connect_port('state', 'pom/frame/robot');
string = ['Initializing maneuver: ',result.status];
disp(string);

%% %%%%%%%%%%%%% BATTERY %%%%%%%%%%%%%%%%%%%%
batteryLevel = rotorcraft.get_battery();
disp(strcat('Battery level: ', num2str(batteryLevel.result.battery.level)));