%rinomina il file est_T2.m
function [off_T ,men_theo_T , men_eff_T]=est_T2(Arot, XCogWet, XCogWet_dist , Tmen)

XCog    = XCogWet;


men_ring_radius = 0.340; % m

% Calculate thruster position with respect to CoG
men_thr1_pos0   = men_ring_radius*[ 1/sqrt(2), 1/sqrt(2),0.0 ]';
men_thr2_pos0   = men_ring_radius*[ 1/sqrt(2),-1/sqrt(2),0.0 ]';
men_thr3_pos0   = men_ring_radius*[-1/sqrt(2),-1/sqrt(2),0.0 ]';
men_thr4_pos0   = men_ring_radius*[-1/sqrt(2), 1/sqrt(2),0.0 ]';

men_thr1_pos   = Arot*men_thr1_pos0 - XCog;
men_thr2_pos   = Arot*men_thr2_pos0 - XCog;
men_thr3_pos   = Arot*men_thr3_pos0 - XCog;
men_thr4_pos   = Arot*men_thr4_pos0 - XCog;

men_cant_angle = 10*pi/180;   % radians  %MEN z-axis aligned

% Directions (unit force vectors)
men_thr1_dir  = Arot*[  sin(-men_cant_angle),  0.0, cos(men_cant_angle) ]';
men_thr2_dir  = Arot*[  sin(-men_cant_angle),  0.0, cos(men_cant_angle) ]';
men_thr3_dir  = Arot*[  sin(men_cant_angle), 0.0, cos(men_cant_angle) ]';
men_thr4_dir  = Arot*[  sin(men_cant_angle), 0.0, cos(men_cant_angle) ]';

% MEN Thrust vectors

men_thr1_torque = Tmen*cross(men_thr1_pos, men_thr1_dir);
men_thr2_torque = Tmen*cross(men_thr2_pos, men_thr2_dir);
men_thr3_torque = Tmen*cross(men_thr3_pos, men_thr3_dir);
men_thr4_torque = Tmen*cross(men_thr4_pos, men_thr4_dir);

men_theo_T=[men_thr1_torque,men_thr2_torque,men_thr3_torque,men_thr4_torque];

off_T=[-men_thr1_torque , -men_thr2_torque , -men_thr3_torque , -men_thr4_torque]; %serve per Brot

% Calculate thruster position with respect to CoG
men_thr1_pos_d   = Arot*men_thr1_pos0 - XCogWet_dist;
men_thr2_pos_d   = Arot*men_thr2_pos0 - XCogWet_dist;
men_thr3_pos_d   = Arot*men_thr3_pos0 - XCogWet_dist;
men_thr4_pos_d   = Arot*men_thr4_pos0 - XCogWet_dist;

%MEN parassite torque vector
men_thr1_torque_p = Tmen*cross(men_thr1_pos_d, men_thr1_dir);
men_thr2_torque_p = Tmen*cross(men_thr2_pos_d, men_thr2_dir);
men_thr3_torque_p= Tmen*cross(men_thr3_pos_d, men_thr3_dir);
men_thr4_torque_p= Tmen*cross(men_thr4_pos_d, men_thr4_dir);

men_eff_T=[men_thr1_torque_p , men_thr2_torque_p , men_thr3_torque_p , men_thr4_torque_p];