load advirgo1_optmat_dd_hp.mat

%%%%%%%%%%%%%%
% ASY
%
% SB1: DARM
% 2M1: MICH
% 3P1: PRCL
%

freq = 100;
idx = min(find(f>freq));

ASYp_SB1_phi = real( atan(DARM_ASYp_SB1_Q(idx) / DARM_ASYp_SB1_P(idx)) );
disp(['ASYp_SB1_PHASE = ', num2str(180/pi * ASYp_SB1_phi), ';']);

DARM_ASYp_SB1_PP =  cos(ASYp_SB1_phi) * DARM_ASYp_SB1_P + sin(ASYp_SB1_phi) * DARM_ASYp_SB1_Q;
DARM_ASYp_SB1_QQ = -sin(ASYp_SB1_phi) * DARM_ASYp_SB1_P + cos(ASYp_SB1_phi) * DARM_ASYp_SB1_Q;
CARM_ASYp_SB1_PP =  cos(ASYp_SB1_phi) * CARM_ASYp_SB1_P + sin(ASYp_SB1_phi) * CARM_ASYp_SB1_Q;
CARM_ASYp_SB1_QQ = -sin(ASYp_SB1_phi) * CARM_ASYp_SB1_P + cos(ASYp_SB1_phi) * CARM_ASYp_SB1_Q;
MICH_ASYp_SB1_PP =  cos(ASYp_SB1_phi) * MICH_ASYp_SB1_P + sin(ASYp_SB1_phi) * MICH_ASYp_SB1_Q;
MICH_ASYp_SB1_QQ = -sin(ASYp_SB1_phi) * MICH_ASYp_SB1_P + cos(ASYp_SB1_phi) * MICH_ASYp_SB1_Q;
PRCL_ASYp_SB1_PP =  cos(ASYp_SB1_phi) * PRCL_ASYp_SB1_P + sin(ASYp_SB1_phi) * PRCL_ASYp_SB1_Q;
PRCL_ASYp_SB1_QQ = -sin(ASYp_SB1_phi) * PRCL_ASYp_SB1_P + cos(ASYp_SB1_phi) * PRCL_ASYp_SB1_Q;
SRCL_ASYp_SB1_PP =  cos(ASYp_SB1_phi) * SRCL_ASYp_SB1_P + sin(ASYp_SB1_phi) * SRCL_ASYp_SB1_Q;
SRCL_ASYp_SB1_QQ = -sin(ASYp_SB1_phi) * SRCL_ASYp_SB1_P + cos(ASYp_SB1_phi) * SRCL_ASYp_SB1_Q;

ASYp_2M1_phi = real( atan(MICH_ASYp_2M1_Q(idx) / MICH_ASYp_2M1_P(idx)) );
disp(['ASYp_2M1_PHASE = ', num2str(180/pi * ASYp_2M1_phi), ';']);

DARM_ASYp_2M1_PP =  cos(ASYp_2M1_phi) * DARM_ASYp_2M1_P + sin(ASYp_2M1_phi) * DARM_ASYp_2M1_Q;
DARM_ASYp_2M1_QQ = -sin(ASYp_2M1_phi) * DARM_ASYp_2M1_P + cos(ASYp_2M1_phi) * DARM_ASYp_2M1_Q;
CARM_ASYp_2M1_PP =  cos(ASYp_2M1_phi) * CARM_ASYp_2M1_P + sin(ASYp_2M1_phi) * CARM_ASYp_2M1_Q;
CARM_ASYp_2M1_QQ = -sin(ASYp_2M1_phi) * CARM_ASYp_2M1_P + cos(ASYp_2M1_phi) * CARM_ASYp_2M1_Q;
MICH_ASYp_2M1_PP =  cos(ASYp_2M1_phi) * MICH_ASYp_2M1_P + sin(ASYp_2M1_phi) * MICH_ASYp_2M1_Q;
MICH_ASYp_2M1_QQ = -sin(ASYp_2M1_phi) * MICH_ASYp_2M1_P + cos(ASYp_2M1_phi) * MICH_ASYp_2M1_Q;
PRCL_ASYp_2M1_PP =  cos(ASYp_2M1_phi) * PRCL_ASYp_2M1_P + sin(ASYp_2M1_phi) * PRCL_ASYp_2M1_Q;
PRCL_ASYp_2M1_QQ = -sin(ASYp_2M1_phi) * PRCL_ASYp_2M1_P + cos(ASYp_2M1_phi) * PRCL_ASYp_2M1_Q;
SRCL_ASYp_2M1_PP =  cos(ASYp_2M1_phi) * SRCL_ASYp_2M1_P + sin(ASYp_2M1_phi) * SRCL_ASYp_2M1_Q;
SRCL_ASYp_2M1_QQ = -sin(ASYp_2M1_phi) * SRCL_ASYp_2M1_P + cos(ASYp_2M1_phi) * SRCL_ASYp_2M1_Q;

ASYp_3P1_phi = real( atan(PRCL_ASYp_3P1_Q(idx) / PRCL_ASYp_3P1_P(idx)) );
disp(['ASYp_3P1_PHASE = ', num2str(180/pi * ASYp_3P1_phi), ';']);

DARM_ASYp_3P1_PP =  cos(ASYp_3P1_phi) * DARM_ASYp_3P1_P + sin(ASYp_3P1_phi) * DARM_ASYp_3P1_Q;
DARM_ASYp_3P1_QQ = -sin(ASYp_3P1_phi) * DARM_ASYp_3P1_P + cos(ASYp_3P1_phi) * DARM_ASYp_3P1_Q;
CARM_ASYp_3P1_PP =  cos(ASYp_3P1_phi) * CARM_ASYp_3P1_P + sin(ASYp_3P1_phi) * CARM_ASYp_3P1_Q;
CARM_ASYp_3P1_QQ = -sin(ASYp_3P1_phi) * CARM_ASYp_3P1_P + cos(ASYp_3P1_phi) * CARM_ASYp_3P1_Q;
MICH_ASYp_3P1_PP =  cos(ASYp_3P1_phi) * MICH_ASYp_3P1_P + sin(ASYp_3P1_phi) * MICH_ASYp_3P1_Q;
MICH_ASYp_3P1_QQ = -sin(ASYp_3P1_phi) * MICH_ASYp_3P1_P + cos(ASYp_3P1_phi) * MICH_ASYp_3P1_Q;
PRCL_ASYp_3P1_PP =  cos(ASYp_3P1_phi) * PRCL_ASYp_3P1_P + sin(ASYp_3P1_phi) * PRCL_ASYp_3P1_Q;
PRCL_ASYp_3P1_QQ = -sin(ASYp_3P1_phi) * PRCL_ASYp_3P1_P + cos(ASYp_3P1_phi) * PRCL_ASYp_3P1_Q;
SRCL_ASYp_3P1_PP =  cos(ASYp_3P1_phi) * SRCL_ASYp_3P1_P + sin(ASYp_3P1_phi) * SRCL_ASYp_3P1_Q;
SRCL_ASYp_3P1_QQ = -sin(ASYp_3P1_phi) * SRCL_ASYp_3P1_P + cos(ASYp_3P1_phi) * SRCL_ASYp_3P1_Q;

%%%%%%%%%%%%%%
% SYM
%
% SB1: CARM
% 2M1: SRCL
% 3P1: PRCL
%

freq = 100;
idx = min(find(f>freq));

SYM_SB1_phi = real( atan(CARM_SYM_SB1_Q(idx) / CARM_SYM_SB1_P(idx)) );
disp(['SYM_SB1_PHASE = ', num2str(180/pi * SYM_SB1_phi), ';']);

DARM_SYM_SB1_PP =  cos(SYM_SB1_phi) * DARM_SYM_SB1_P + sin(SYM_SB1_phi) * DARM_SYM_SB1_Q;
DARM_SYM_SB1_QQ = -sin(SYM_SB1_phi) * DARM_SYM_SB1_P + cos(SYM_SB1_phi) * DARM_SYM_SB1_Q;
CARM_SYM_SB1_PP =  cos(SYM_SB1_phi) * CARM_SYM_SB1_P + sin(SYM_SB1_phi) * CARM_SYM_SB1_Q;
CARM_SYM_SB1_QQ = -sin(SYM_SB1_phi) * CARM_SYM_SB1_P + cos(SYM_SB1_phi) * CARM_SYM_SB1_Q;
MICH_SYM_SB1_PP =  cos(SYM_SB1_phi) * MICH_SYM_SB1_P + sin(SYM_SB1_phi) * MICH_SYM_SB1_Q;
MICH_SYM_SB1_QQ = -sin(SYM_SB1_phi) * MICH_SYM_SB1_P + cos(SYM_SB1_phi) * MICH_SYM_SB1_Q;
PRCL_SYM_SB1_PP =  cos(SYM_SB1_phi) * PRCL_SYM_SB1_P + sin(SYM_SB1_phi) * PRCL_SYM_SB1_Q;
PRCL_SYM_SB1_QQ = -sin(SYM_SB1_phi) * PRCL_SYM_SB1_P + cos(SYM_SB1_phi) * PRCL_SYM_SB1_Q;
SRCL_SYM_SB1_PP =  cos(SYM_SB1_phi) * SRCL_SYM_SB1_P + sin(SYM_SB1_phi) * SRCL_SYM_SB1_Q;
SRCL_SYM_SB1_QQ = -sin(SYM_SB1_phi) * SRCL_SYM_SB1_P + cos(SYM_SB1_phi) * SRCL_SYM_SB1_Q;

SYM_2M1_phi = real( atan(SRCL_SYM_2M1_Q(idx) / SRCL_SYM_2M1_P(idx)) );
disp(['SYM_2M1_PHASE = ', num2str(180/pi * SYM_2M1_phi), ';']);

DARM_SYM_2M1_PP =  cos(SYM_2M1_phi) * DARM_SYM_2M1_P + sin(SYM_2M1_phi) * DARM_SYM_2M1_Q;
DARM_SYM_2M1_QQ = -sin(SYM_2M1_phi) * DARM_SYM_2M1_P + cos(SYM_2M1_phi) * DARM_SYM_2M1_Q;
CARM_SYM_2M1_PP =  cos(SYM_2M1_phi) * CARM_SYM_2M1_P + sin(SYM_2M1_phi) * CARM_SYM_2M1_Q;
CARM_SYM_2M1_QQ = -sin(SYM_2M1_phi) * CARM_SYM_2M1_P + cos(SYM_2M1_phi) * CARM_SYM_2M1_Q;
MICH_SYM_2M1_PP =  cos(SYM_2M1_phi) * MICH_SYM_2M1_P + sin(SYM_2M1_phi) * MICH_SYM_2M1_Q;
MICH_SYM_2M1_QQ = -sin(SYM_2M1_phi) * MICH_SYM_2M1_P + cos(SYM_2M1_phi) * MICH_SYM_2M1_Q;
PRCL_SYM_2M1_PP =  cos(SYM_2M1_phi) * PRCL_SYM_2M1_P + sin(SYM_2M1_phi) * PRCL_SYM_2M1_Q;
PRCL_SYM_2M1_QQ = -sin(SYM_2M1_phi) * PRCL_SYM_2M1_P + cos(SYM_2M1_phi) * PRCL_SYM_2M1_Q;
SRCL_SYM_2M1_PP =  cos(SYM_2M1_phi) * SRCL_SYM_2M1_P + sin(SYM_2M1_phi) * SRCL_SYM_2M1_Q;
SRCL_SYM_2M1_QQ = -sin(SYM_2M1_phi) * SRCL_SYM_2M1_P + cos(SYM_2M1_phi) * SRCL_SYM_2M1_Q;

SYM_3P1_phi = real( atan(PRCL_SYM_3P1_Q(idx) / PRCL_SYM_3P1_P(idx)) );
disp(['SYM_3P1_PHASE = ', num2str(180/pi * SYM_3P1_phi), ';']);

DARM_SYM_3P1_PP =  cos(SYM_3P1_phi) * DARM_SYM_3P1_P + sin(SYM_3P1_phi) * DARM_SYM_3P1_Q;
DARM_SYM_3P1_QQ = -sin(SYM_3P1_phi) * DARM_SYM_3P1_P + cos(SYM_3P1_phi) * DARM_SYM_3P1_Q;
CARM_SYM_3P1_PP =  cos(SYM_3P1_phi) * CARM_SYM_3P1_P + sin(SYM_3P1_phi) * CARM_SYM_3P1_Q;
CARM_SYM_3P1_QQ = -sin(SYM_3P1_phi) * CARM_SYM_3P1_P + cos(SYM_3P1_phi) * CARM_SYM_3P1_Q;
MICH_SYM_3P1_PP =  cos(SYM_3P1_phi) * MICH_SYM_3P1_P + sin(SYM_3P1_phi) * MICH_SYM_3P1_Q;
MICH_SYM_3P1_QQ = -sin(SYM_3P1_phi) * MICH_SYM_3P1_P + cos(SYM_3P1_phi) * MICH_SYM_3P1_Q;
PRCL_SYM_3P1_PP =  cos(SYM_3P1_phi) * PRCL_SYM_3P1_P + sin(SYM_3P1_phi) * PRCL_SYM_3P1_Q;
PRCL_SYM_3P1_QQ = -sin(SYM_3P1_phi) * PRCL_SYM_3P1_P + cos(SYM_3P1_phi) * PRCL_SYM_3P1_Q;
SRCL_SYM_3P1_PP =  cos(SYM_3P1_phi) * SRCL_SYM_3P1_P + sin(SYM_3P1_phi) * SRCL_SYM_3P1_Q;
SRCL_SYM_3P1_QQ = -sin(SYM_3P1_phi) * SRCL_SYM_3P1_P + cos(SYM_3P1_phi) * SRCL_SYM_3P1_Q;

%%%%%%%%%%%%%%
% POBS
%
% SB1: CARM
% 2M1: SRCL
% 3P1: MICH
%

freq = 100;
idx = min(find(f>freq));

POBS_SB1_phi = real( atan(CARM_POBS_SB1_Q(idx) / CARM_POBS_SB1_P(idx)) );
disp(['POBS_SB1_PHASE = ', num2str(180/pi * POBS_SB1_phi), ';']);

DARM_POBS_SB1_PP =  cos(POBS_SB1_phi) * DARM_POBS_SB1_P + sin(POBS_SB1_phi) * DARM_POBS_SB1_Q;
DARM_POBS_SB1_QQ = -sin(POBS_SB1_phi) * DARM_POBS_SB1_P + cos(POBS_SB1_phi) * DARM_POBS_SB1_Q;
CARM_POBS_SB1_PP =  cos(POBS_SB1_phi) * CARM_POBS_SB1_P + sin(POBS_SB1_phi) * CARM_POBS_SB1_Q;
CARM_POBS_SB1_QQ = -sin(POBS_SB1_phi) * CARM_POBS_SB1_P + cos(POBS_SB1_phi) * CARM_POBS_SB1_Q;
MICH_POBS_SB1_PP =  cos(POBS_SB1_phi) * MICH_POBS_SB1_P + sin(POBS_SB1_phi) * MICH_POBS_SB1_Q;
MICH_POBS_SB1_QQ = -sin(POBS_SB1_phi) * MICH_POBS_SB1_P + cos(POBS_SB1_phi) * MICH_POBS_SB1_Q;
PRCL_POBS_SB1_PP =  cos(POBS_SB1_phi) * PRCL_POBS_SB1_P + sin(POBS_SB1_phi) * PRCL_POBS_SB1_Q;
PRCL_POBS_SB1_QQ = -sin(POBS_SB1_phi) * PRCL_POBS_SB1_P + cos(POBS_SB1_phi) * PRCL_POBS_SB1_Q;
SRCL_POBS_SB1_PP =  cos(POBS_SB1_phi) * SRCL_POBS_SB1_P + sin(POBS_SB1_phi) * SRCL_POBS_SB1_Q;
SRCL_POBS_SB1_QQ = -sin(POBS_SB1_phi) * SRCL_POBS_SB1_P + cos(POBS_SB1_phi) * SRCL_POBS_SB1_Q;

POBS_2M1_phi = real( atan(SRCL_POBS_2M1_Q(idx) / SRCL_POBS_2M1_P(idx)) );
disp(['POBS_2M1_PHASE = ', num2str(180/pi * POBS_2M1_phi), ';']);

DARM_POBS_2M1_PP =  cos(POBS_2M1_phi) * DARM_POBS_2M1_P + sin(POBS_2M1_phi) * DARM_POBS_2M1_Q;
DARM_POBS_2M1_QQ = -sin(POBS_2M1_phi) * DARM_POBS_2M1_P + cos(POBS_2M1_phi) * DARM_POBS_2M1_Q;
CARM_POBS_2M1_PP =  cos(POBS_2M1_phi) * CARM_POBS_2M1_P + sin(POBS_2M1_phi) * CARM_POBS_2M1_Q;
CARM_POBS_2M1_QQ = -sin(POBS_2M1_phi) * CARM_POBS_2M1_P + cos(POBS_2M1_phi) * CARM_POBS_2M1_Q;
MICH_POBS_2M1_PP =  cos(POBS_2M1_phi) * MICH_POBS_2M1_P + sin(POBS_2M1_phi) * MICH_POBS_2M1_Q;
MICH_POBS_2M1_QQ = -sin(POBS_2M1_phi) * MICH_POBS_2M1_P + cos(POBS_2M1_phi) * MICH_POBS_2M1_Q;
PRCL_POBS_2M1_PP =  cos(POBS_2M1_phi) * PRCL_POBS_2M1_P + sin(POBS_2M1_phi) * PRCL_POBS_2M1_Q;
PRCL_POBS_2M1_QQ = -sin(POBS_2M1_phi) * PRCL_POBS_2M1_P + cos(POBS_2M1_phi) * PRCL_POBS_2M1_Q;
SRCL_POBS_2M1_PP =  cos(POBS_2M1_phi) * SRCL_POBS_2M1_P + sin(POBS_2M1_phi) * SRCL_POBS_2M1_Q;
SRCL_POBS_2M1_QQ = -sin(POBS_2M1_phi) * SRCL_POBS_2M1_P + cos(POBS_2M1_phi) * SRCL_POBS_2M1_Q;

POBS_3P1_phi = real( atan(MICH_POBS_3P1_Q(idx) / MICH_POBS_3P1_P(idx)) );;
disp(['POBS_3P1_PHASE = ', num2str(180/pi * POBS_3P1_phi), ';']);

DARM_POBS_3P1_PP =  cos(POBS_3P1_phi) * DARM_POBS_3P1_P + sin(POBS_3P1_phi) * DARM_POBS_3P1_Q;
DARM_POBS_3P1_QQ = -sin(POBS_3P1_phi) * DARM_POBS_3P1_P + cos(POBS_3P1_phi) * DARM_POBS_3P1_Q;
CARM_POBS_3P1_PP =  cos(POBS_3P1_phi) * CARM_POBS_3P1_P + sin(POBS_3P1_phi) * CARM_POBS_3P1_Q;
CARM_POBS_3P1_QQ = -sin(POBS_3P1_phi) * CARM_POBS_3P1_P + cos(POBS_3P1_phi) * CARM_POBS_3P1_Q;
MICH_POBS_3P1_PP =  cos(POBS_3P1_phi) * MICH_POBS_3P1_P + sin(POBS_3P1_phi) * MICH_POBS_3P1_Q;
MICH_POBS_3P1_QQ = -sin(POBS_3P1_phi) * MICH_POBS_3P1_P + cos(POBS_3P1_phi) * MICH_POBS_3P1_Q;
PRCL_POBS_3P1_PP =  cos(POBS_3P1_phi) * PRCL_POBS_3P1_P + sin(POBS_3P1_phi) * PRCL_POBS_3P1_Q;
PRCL_POBS_3P1_QQ = -sin(POBS_3P1_phi) * PRCL_POBS_3P1_P + cos(POBS_3P1_phi) * PRCL_POBS_3P1_Q;
SRCL_POBS_3P1_PP =  cos(POBS_3P1_phi) * SRCL_POBS_3P1_P + sin(POBS_3P1_phi) * SRCL_POBS_3P1_Q;
SRCL_POBS_3P1_QQ = -sin(POBS_3P1_phi) * SRCL_POBS_3P1_P + cos(POBS_3P1_phi) * SRCL_POBS_3P1_Q;

%%%%%%%%%%%%%%
% XP
%
% SB1: DARM
% 2M1: SRCL
% 3P1: MICH
%

freq = 100;
idx = min(find(f>freq));

XP_SB1_phi = real( atan(DARM_XP_SB1_Q(idx) / DARM_XP_SB1_P(idx)) );
disp(['XP_SB1_PHASE = ', num2str(180/pi * XP_SB1_phi), ';']);

DARM_XP_SB1_PP =  cos(XP_SB1_phi) * DARM_XP_SB1_P + sin(XP_SB1_phi) * DARM_XP_SB1_Q;
DARM_XP_SB1_QQ = -sin(XP_SB1_phi) * DARM_XP_SB1_P + cos(XP_SB1_phi) * DARM_XP_SB1_Q;
CARM_XP_SB1_PP =  cos(XP_SB1_phi) * CARM_XP_SB1_P + sin(XP_SB1_phi) * CARM_XP_SB1_Q;
CARM_XP_SB1_QQ = -sin(XP_SB1_phi) * CARM_XP_SB1_P + cos(XP_SB1_phi) * CARM_XP_SB1_Q;
MICH_XP_SB1_PP =  cos(XP_SB1_phi) * MICH_XP_SB1_P + sin(XP_SB1_phi) * MICH_XP_SB1_Q;
MICH_XP_SB1_QQ = -sin(XP_SB1_phi) * MICH_XP_SB1_P + cos(XP_SB1_phi) * MICH_XP_SB1_Q;
PRCL_XP_SB1_PP =  cos(XP_SB1_phi) * PRCL_XP_SB1_P + sin(XP_SB1_phi) * PRCL_XP_SB1_Q;
PRCL_XP_SB1_QQ = -sin(XP_SB1_phi) * PRCL_XP_SB1_P + cos(XP_SB1_phi) * PRCL_XP_SB1_Q;
SRCL_XP_SB1_PP =  cos(XP_SB1_phi) * SRCL_XP_SB1_P + sin(XP_SB1_phi) * SRCL_XP_SB1_Q;
SRCL_XP_SB1_QQ = -sin(XP_SB1_phi) * SRCL_XP_SB1_P + cos(XP_SB1_phi) * SRCL_XP_SB1_Q;

XP_2M1_phi = real( atan(SRCL_XP_2M1_Q(idx) / SRCL_XP_2M1_P(idx)) );
disp(['XP_2M1_PHASE = ', num2str(180/pi * XP_2M1_phi), ';']);

DARM_XP_2M1_PP =  cos(XP_2M1_phi) * DARM_XP_2M1_P + sin(XP_2M1_phi) * DARM_XP_2M1_Q;
DARM_XP_2M1_QQ = -sin(XP_2M1_phi) * DARM_XP_2M1_P + cos(XP_2M1_phi) * DARM_XP_2M1_Q;
CARM_XP_2M1_PP =  cos(XP_2M1_phi) * CARM_XP_2M1_P + sin(XP_2M1_phi) * CARM_XP_2M1_Q;
CARM_XP_2M1_QQ = -sin(XP_2M1_phi) * CARM_XP_2M1_P + cos(XP_2M1_phi) * CARM_XP_2M1_Q;
MICH_XP_2M1_PP =  cos(XP_2M1_phi) * MICH_XP_2M1_P + sin(XP_2M1_phi) * MICH_XP_2M1_Q;
MICH_XP_2M1_QQ = -sin(XP_2M1_phi) * MICH_XP_2M1_P + cos(XP_2M1_phi) * MICH_XP_2M1_Q;
PRCL_XP_2M1_PP =  cos(XP_2M1_phi) * PRCL_XP_2M1_P + sin(XP_2M1_phi) * PRCL_XP_2M1_Q;
PRCL_XP_2M1_QQ = -sin(XP_2M1_phi) * PRCL_XP_2M1_P + cos(XP_2M1_phi) * PRCL_XP_2M1_Q;
SRCL_XP_2M1_PP =  cos(XP_2M1_phi) * SRCL_XP_2M1_P + sin(XP_2M1_phi) * SRCL_XP_2M1_Q;
SRCL_XP_2M1_QQ = -sin(XP_2M1_phi) * SRCL_XP_2M1_P + cos(XP_2M1_phi) * SRCL_XP_2M1_Q;

XP_3P1_phi = real( atan(MICH_XP_3P1_Q(idx) / MICH_XP_3P1_P(idx)) );
disp(['XP_3P1_PHASE = ', num2str(180/pi * XP_3P1_phi), ';']);

DARM_XP_3P1_PP =  cos(XP_3P1_phi) * DARM_XP_3P1_P + sin(XP_3P1_phi) * DARM_XP_3P1_Q;
DARM_XP_3P1_QQ = -sin(XP_3P1_phi) * DARM_XP_3P1_P + cos(XP_3P1_phi) * DARM_XP_3P1_Q;
CARM_XP_3P1_PP =  cos(XP_3P1_phi) * CARM_XP_3P1_P + sin(XP_3P1_phi) * CARM_XP_3P1_Q;
CARM_XP_3P1_QQ = -sin(XP_3P1_phi) * CARM_XP_3P1_P + cos(XP_3P1_phi) * CARM_XP_3P1_Q;
MICH_XP_3P1_PP =  cos(XP_3P1_phi) * MICH_XP_3P1_P + sin(XP_3P1_phi) * MICH_XP_3P1_Q;
MICH_XP_3P1_QQ = -sin(XP_3P1_phi) * MICH_XP_3P1_P + cos(XP_3P1_phi) * MICH_XP_3P1_Q;
PRCL_XP_3P1_PP =  cos(XP_3P1_phi) * PRCL_XP_3P1_P + sin(XP_3P1_phi) * PRCL_XP_3P1_Q;
PRCL_XP_3P1_QQ = -sin(XP_3P1_phi) * PRCL_XP_3P1_P + cos(XP_3P1_phi) * PRCL_XP_3P1_Q;
SRCL_XP_3P1_PP =  cos(XP_3P1_phi) * SRCL_XP_3P1_P + sin(XP_3P1_phi) * SRCL_XP_3P1_Q;
SRCL_XP_3P1_QQ = -sin(XP_3P1_phi) * SRCL_XP_3P1_P + cos(XP_3P1_phi) * SRCL_XP_3P1_Q;

%%%%%%%%%%%%%%
% YP
%
% SB1: DARM
% 2M1: SRCL
% 3P1: MICH
%

freq = 100;
idx = min(find(f>freq));

YP_SB1_phi = real( atan(DARM_YP_SB1_Q(idx) / DARM_YP_SB1_P(idx)) );
disp(['YP_SB1_PHASE = ', num2str(180/pi * YP_SB1_phi), ';']);

DARM_YP_SB1_PP =  cos(YP_SB1_phi) * DARM_YP_SB1_P + sin(YP_SB1_phi) * DARM_YP_SB1_Q;
DARM_YP_SB1_QQ = -sin(YP_SB1_phi) * DARM_YP_SB1_P + cos(YP_SB1_phi) * DARM_YP_SB1_Q;
CARM_YP_SB1_PP =  cos(YP_SB1_phi) * CARM_YP_SB1_P + sin(YP_SB1_phi) * CARM_YP_SB1_Q;
CARM_YP_SB1_QQ = -sin(YP_SB1_phi) * CARM_YP_SB1_P + cos(YP_SB1_phi) * CARM_YP_SB1_Q;
MICH_YP_SB1_PP =  cos(YP_SB1_phi) * MICH_YP_SB1_P + sin(YP_SB1_phi) * MICH_YP_SB1_Q;
MICH_YP_SB1_QQ = -sin(YP_SB1_phi) * MICH_YP_SB1_P + cos(YP_SB1_phi) * MICH_YP_SB1_Q;
PRCL_YP_SB1_PP =  cos(YP_SB1_phi) * PRCL_YP_SB1_P + sin(YP_SB1_phi) * PRCL_YP_SB1_Q;
PRCL_YP_SB1_QQ = -sin(YP_SB1_phi) * PRCL_YP_SB1_P + cos(YP_SB1_phi) * PRCL_YP_SB1_Q;
SRCL_YP_SB1_PP =  cos(YP_SB1_phi) * SRCL_YP_SB1_P + sin(YP_SB1_phi) * SRCL_YP_SB1_Q;
SRCL_YP_SB1_QQ = -sin(YP_SB1_phi) * SRCL_YP_SB1_P + cos(YP_SB1_phi) * SRCL_YP_SB1_Q;

YP_2M1_phi = real( atan(SRCL_YP_2M1_Q(idx) / SRCL_YP_2M1_P(idx)) );
disp(['YP_2M1_PHASE = ', num2str(180/pi * YP_2M1_phi), ';']);

DARM_YP_2M1_PP =  cos(YP_2M1_phi) * DARM_YP_2M1_P + sin(YP_2M1_phi) * DARM_YP_2M1_Q;
DARM_YP_2M1_QQ = -sin(YP_2M1_phi) * DARM_YP_2M1_P + cos(YP_2M1_phi) * DARM_YP_2M1_Q;
CARM_YP_2M1_PP =  cos(YP_2M1_phi) * CARM_YP_2M1_P + sin(YP_2M1_phi) * CARM_YP_2M1_Q;
CARM_YP_2M1_QQ = -sin(YP_2M1_phi) * CARM_YP_2M1_P + cos(YP_2M1_phi) * CARM_YP_2M1_Q;
MICH_YP_2M1_PP =  cos(YP_2M1_phi) * MICH_YP_2M1_P + sin(YP_2M1_phi) * MICH_YP_2M1_Q;
MICH_YP_2M1_QQ = -sin(YP_2M1_phi) * MICH_YP_2M1_P + cos(YP_2M1_phi) * MICH_YP_2M1_Q;
PRCL_YP_2M1_PP =  cos(YP_2M1_phi) * PRCL_YP_2M1_P + sin(YP_2M1_phi) * PRCL_YP_2M1_Q;
PRCL_YP_2M1_QQ = -sin(YP_2M1_phi) * PRCL_YP_2M1_P + cos(YP_2M1_phi) * PRCL_YP_2M1_Q;
SRCL_YP_2M1_PP =  cos(YP_2M1_phi) * SRCL_YP_2M1_P + sin(YP_2M1_phi) * SRCL_YP_2M1_Q;
SRCL_YP_2M1_QQ = -sin(YP_2M1_phi) * SRCL_YP_2M1_P + cos(YP_2M1_phi) * SRCL_YP_2M1_Q;

YP_3P1_phi = real( atan(MICH_YP_3P1_Q(idx) / MICH_YP_3P1_P(idx)) );
disp(['YP_3P1_PHASE = ', num2str(180/pi * YP_3P1_phi), ';']);

DARM_YP_3P1_PP =  cos(YP_3P1_phi) * DARM_YP_3P1_P + sin(YP_3P1_phi) * DARM_YP_3P1_Q;
DARM_YP_3P1_QQ = -sin(YP_3P1_phi) * DARM_YP_3P1_P + cos(YP_3P1_phi) * DARM_YP_3P1_Q;
CARM_YP_3P1_PP =  cos(YP_3P1_phi) * CARM_YP_3P1_P + sin(YP_3P1_phi) * CARM_YP_3P1_Q;
CARM_YP_3P1_QQ = -sin(YP_3P1_phi) * CARM_YP_3P1_P + cos(YP_3P1_phi) * CARM_YP_3P1_Q;
MICH_YP_3P1_PP =  cos(YP_3P1_phi) * MICH_YP_3P1_P + sin(YP_3P1_phi) * MICH_YP_3P1_Q;
MICH_YP_3P1_QQ = -sin(YP_3P1_phi) * MICH_YP_3P1_P + cos(YP_3P1_phi) * MICH_YP_3P1_Q;
PRCL_YP_3P1_PP =  cos(YP_3P1_phi) * PRCL_YP_3P1_P + sin(YP_3P1_phi) * PRCL_YP_3P1_Q;
PRCL_YP_3P1_QQ = -sin(YP_3P1_phi) * PRCL_YP_3P1_P + cos(YP_3P1_phi) * PRCL_YP_3P1_Q;
SRCL_YP_3P1_PP =  cos(YP_3P1_phi) * SRCL_YP_3P1_P + sin(YP_3P1_phi) * SRCL_YP_3P1_Q;
SRCL_YP_3P1_QQ = -sin(YP_3P1_phi) * SRCL_YP_3P1_P + cos(YP_3P1_phi) * SRCL_YP_3P1_Q;

%%%%%%%%%%%%%%
% POX
%
% SB1: CARM
% 2M1: SRCL
% 3P1: MICH
%

freq = 100;
idx = min(find(f>freq));

POX_SB1_phi = real( atan(CARM_POX_SB1_Q(idx) / CARM_POX_SB1_P(idx)) );
disp(['POX_SB1_PHASE = ', num2str(180/pi * POX_SB1_phi), ';']);

DARM_POX_SB1_PP =  cos(POX_SB1_phi) * DARM_POX_SB1_P + sin(POX_SB1_phi) * DARM_POX_SB1_Q;
DARM_POX_SB1_QQ = -sin(POX_SB1_phi) * DARM_POX_SB1_P + cos(POX_SB1_phi) * DARM_POX_SB1_Q;
CARM_POX_SB1_PP =  cos(POX_SB1_phi) * CARM_POX_SB1_P + sin(POX_SB1_phi) * CARM_POX_SB1_Q;
CARM_POX_SB1_QQ = -sin(POX_SB1_phi) * CARM_POX_SB1_P + cos(POX_SB1_phi) * CARM_POX_SB1_Q;
MICH_POX_SB1_PP =  cos(POX_SB1_phi) * MICH_POX_SB1_P + sin(POX_SB1_phi) * MICH_POX_SB1_Q;
MICH_POX_SB1_QQ = -sin(POX_SB1_phi) * MICH_POX_SB1_P + cos(POX_SB1_phi) * MICH_POX_SB1_Q;
PRCL_POX_SB1_PP =  cos(POX_SB1_phi) * PRCL_POX_SB1_P + sin(POX_SB1_phi) * PRCL_POX_SB1_Q;
PRCL_POX_SB1_QQ = -sin(POX_SB1_phi) * PRCL_POX_SB1_P + cos(POX_SB1_phi) * PRCL_POX_SB1_Q;
SRCL_POX_SB1_PP =  cos(POX_SB1_phi) * SRCL_POX_SB1_P + sin(POX_SB1_phi) * SRCL_POX_SB1_Q;
SRCL_POX_SB1_QQ = -sin(POX_SB1_phi) * SRCL_POX_SB1_P + cos(POX_SB1_phi) * SRCL_POX_SB1_Q;

POX_2M1_phi = real( atan(SRCL_POX_2M1_Q(idx) / SRCL_POX_2M1_P(idx)) );
disp(['POX_2M1_PHASE = ', num2str(180/pi * POX_2M1_phi), ';']);

DARM_POX_2M1_PP =  cos(POX_2M1_phi) * DARM_POX_2M1_P + sin(POX_2M1_phi) * DARM_POX_2M1_Q;
DARM_POX_2M1_QQ = -sin(POX_2M1_phi) * DARM_POX_2M1_P + cos(POX_2M1_phi) * DARM_POX_2M1_Q;
CARM_POX_2M1_PP =  cos(POX_2M1_phi) * CARM_POX_2M1_P + sin(POX_2M1_phi) * CARM_POX_2M1_Q;
CARM_POX_2M1_QQ = -sin(POX_2M1_phi) * CARM_POX_2M1_P + cos(POX_2M1_phi) * CARM_POX_2M1_Q;
MICH_POX_2M1_PP =  cos(POX_2M1_phi) * MICH_POX_2M1_P + sin(POX_2M1_phi) * MICH_POX_2M1_Q;
MICH_POX_2M1_QQ = -sin(POX_2M1_phi) * MICH_POX_2M1_P + cos(POX_2M1_phi) * MICH_POX_2M1_Q;
PRCL_POX_2M1_PP =  cos(POX_2M1_phi) * PRCL_POX_2M1_P + sin(POX_2M1_phi) * PRCL_POX_2M1_Q;
PRCL_POX_2M1_QQ = -sin(POX_2M1_phi) * PRCL_POX_2M1_P + cos(POX_2M1_phi) * PRCL_POX_2M1_Q;
SRCL_POX_2M1_PP =  cos(POX_2M1_phi) * SRCL_POX_2M1_P + sin(POX_2M1_phi) * SRCL_POX_2M1_Q;
SRCL_POX_2M1_QQ = -sin(POX_2M1_phi) * SRCL_POX_2M1_P + cos(POX_2M1_phi) * SRCL_POX_2M1_Q;

POX_3P1_phi = real( atan(MICH_POX_3P1_Q(idx) / MICH_POX_3P1_P(idx)) );
disp(['POX_3P1_PHASE = ', num2str(180/pi * POX_3P1_phi), ';']);

DARM_POX_3P1_PP =  cos(POX_3P1_phi) * DARM_POX_3P1_P + sin(POX_3P1_phi) * DARM_POX_3P1_Q;
DARM_POX_3P1_QQ = -sin(POX_3P1_phi) * DARM_POX_3P1_P + cos(POX_3P1_phi) * DARM_POX_3P1_Q;
CARM_POX_3P1_PP =  cos(POX_3P1_phi) * CARM_POX_3P1_P + sin(POX_3P1_phi) * CARM_POX_3P1_Q;
CARM_POX_3P1_QQ = -sin(POX_3P1_phi) * CARM_POX_3P1_P + cos(POX_3P1_phi) * CARM_POX_3P1_Q;
MICH_POX_3P1_PP =  cos(POX_3P1_phi) * MICH_POX_3P1_P + sin(POX_3P1_phi) * MICH_POX_3P1_Q;
MICH_POX_3P1_QQ = -sin(POX_3P1_phi) * MICH_POX_3P1_P + cos(POX_3P1_phi) * MICH_POX_3P1_Q;
PRCL_POX_3P1_PP =  cos(POX_3P1_phi) * PRCL_POX_3P1_P + sin(POX_3P1_phi) * PRCL_POX_3P1_Q;
PRCL_POX_3P1_QQ = -sin(POX_3P1_phi) * PRCL_POX_3P1_P + cos(POX_3P1_phi) * PRCL_POX_3P1_Q;
SRCL_POX_3P1_PP =  cos(POX_3P1_phi) * SRCL_POX_3P1_P + sin(POX_3P1_phi) * SRCL_POX_3P1_Q;
SRCL_POX_3P1_QQ = -sin(POX_3P1_phi) * SRCL_POX_3P1_P + cos(POX_3P1_phi) * SRCL_POX_3P1_Q;

%%%%%%%%%%%%%%
% POY
%
% SB1: CARM
% 2M1: SRCL
% 3P1: MICH
%

freq = 100;
idx = min(find(f>freq));

POY_SB1_phi = real( atan(CARM_POY_SB1_Q(idx) / CARM_POY_SB1_P(idx)) );
disp(['POY_SB1_PHASE = ', num2str(180/pi * POY_SB1_phi), ';']);

DARM_POY_SB1_PP =  cos(POY_SB1_phi) * DARM_POY_SB1_P + sin(POY_SB1_phi) * DARM_POY_SB1_Q;
DARM_POY_SB1_QQ = -sin(POY_SB1_phi) * DARM_POY_SB1_P + cos(POY_SB1_phi) * DARM_POY_SB1_Q;
CARM_POY_SB1_PP =  cos(POY_SB1_phi) * CARM_POY_SB1_P + sin(POY_SB1_phi) * CARM_POY_SB1_Q;
CARM_POY_SB1_QQ = -sin(POY_SB1_phi) * CARM_POY_SB1_P + cos(POY_SB1_phi) * CARM_POY_SB1_Q;
MICH_POY_SB1_PP =  cos(POY_SB1_phi) * MICH_POY_SB1_P + sin(POY_SB1_phi) * MICH_POY_SB1_Q;
MICH_POY_SB1_QQ = -sin(POY_SB1_phi) * MICH_POY_SB1_P + cos(POY_SB1_phi) * MICH_POY_SB1_Q;
PRCL_POY_SB1_PP =  cos(POY_SB1_phi) * PRCL_POY_SB1_P + sin(POY_SB1_phi) * PRCL_POY_SB1_Q;
PRCL_POY_SB1_QQ = -sin(POY_SB1_phi) * PRCL_POY_SB1_P + cos(POY_SB1_phi) * PRCL_POY_SB1_Q;
SRCL_POY_SB1_PP =  cos(POY_SB1_phi) * SRCL_POY_SB1_P + sin(POY_SB1_phi) * SRCL_POY_SB1_Q;
SRCL_POY_SB1_QQ = -sin(POY_SB1_phi) * SRCL_POY_SB1_P + cos(POY_SB1_phi) * SRCL_POY_SB1_Q;

POY_2M1_phi = real( atan(SRCL_POY_2M1_Q(idx) / SRCL_POY_2M1_P(idx)) );
disp(['POY_2M1_PHASE = ', num2str(180/pi * POY_2M1_phi), ';']);

DARM_POY_2M1_PP =  cos(POY_2M1_phi) * DARM_POY_2M1_P + sin(POY_2M1_phi) * DARM_POY_2M1_Q;
DARM_POY_2M1_QQ = -sin(POY_2M1_phi) * DARM_POY_2M1_P + cos(POY_2M1_phi) * DARM_POY_2M1_Q;
CARM_POY_2M1_PP =  cos(POY_2M1_phi) * CARM_POY_2M1_P + sin(POY_2M1_phi) * CARM_POY_2M1_Q;
CARM_POY_2M1_QQ = -sin(POY_2M1_phi) * CARM_POY_2M1_P + cos(POY_2M1_phi) * CARM_POY_2M1_Q;
MICH_POY_2M1_PP =  cos(POY_2M1_phi) * MICH_POY_2M1_P + sin(POY_2M1_phi) * MICH_POY_2M1_Q;
MICH_POY_2M1_QQ = -sin(POY_2M1_phi) * MICH_POY_2M1_P + cos(POY_2M1_phi) * MICH_POY_2M1_Q;
PRCL_POY_2M1_PP =  cos(POY_2M1_phi) * PRCL_POY_2M1_P + sin(POY_2M1_phi) * PRCL_POY_2M1_Q;
PRCL_POY_2M1_QQ = -sin(POY_2M1_phi) * PRCL_POY_2M1_P + cos(POY_2M1_phi) * PRCL_POY_2M1_Q;
SRCL_POY_2M1_PP =  cos(POY_2M1_phi) * SRCL_POY_2M1_P + sin(POY_2M1_phi) * SRCL_POY_2M1_Q;
SRCL_POY_2M1_QQ = -sin(POY_2M1_phi) * SRCL_POY_2M1_P + cos(POY_2M1_phi) * SRCL_POY_2M1_Q;

POY_3P1_phi = real( atan(MICH_POY_3P1_Q(idx) / MICH_POY_3P1_P(idx)) );
disp(['POY_3P1_PHASE = ', num2str(180/pi * POY_3P1_phi), ';']);

DARM_POY_3P1_PP =  cos(POY_3P1_phi) * DARM_POY_3P1_P + sin(POY_3P1_phi) * DARM_POY_3P1_Q;
DARM_POY_3P1_QQ = -sin(POY_3P1_phi) * DARM_POY_3P1_P + cos(POY_3P1_phi) * DARM_POY_3P1_Q;
CARM_POY_3P1_PP =  cos(POY_3P1_phi) * CARM_POY_3P1_P + sin(POY_3P1_phi) * CARM_POY_3P1_Q;
CARM_POY_3P1_QQ = -sin(POY_3P1_phi) * CARM_POY_3P1_P + cos(POY_3P1_phi) * CARM_POY_3P1_Q;
MICH_POY_3P1_PP =  cos(POY_3P1_phi) * MICH_POY_3P1_P + sin(POY_3P1_phi) * MICH_POY_3P1_Q;
MICH_POY_3P1_QQ = -sin(POY_3P1_phi) * MICH_POY_3P1_P + cos(POY_3P1_phi) * MICH_POY_3P1_Q;
PRCL_POY_3P1_PP =  cos(POY_3P1_phi) * PRCL_POY_3P1_P + sin(POY_3P1_phi) * PRCL_POY_3P1_Q;
PRCL_POY_3P1_QQ = -sin(POY_3P1_phi) * PRCL_POY_3P1_P + cos(POY_3P1_phi) * PRCL_POY_3P1_Q;
SRCL_POY_3P1_PP =  cos(POY_3P1_phi) * SRCL_POY_3P1_P + sin(POY_3P1_phi) * SRCL_POY_3P1_Q;
SRCL_POY_3P1_QQ = -sin(POY_3P1_phi) * SRCL_POY_3P1_P + cos(POY_3P1_phi) * SRCL_POY_3P1_Q;


% save results
save advirgo1_optmat_dd_hp_tuned.mat f *_PP *_QQ *_phi *ASY_DC
