function [m_eml m_vml] = fn_ml_N2psi_NKbeta_Nsgm_with_selection(m_y, a_x, m_Wm, m_Wp, concentrated, v_initial_point, v_set)
[N T] = size(m_y);
K = size(a_x, 3);
m_ysm = m_Wm * m_y;
m_ysp = m_Wp * m_y;

options = optimset('GradObj', 'on', 'Hessian', 'on');
warning off

v_ap = (v_set == 2) | (v_set == 4); % equal to 1 if psi0+ is set to zero
v_am = (v_set == 3) | (v_set == 4); % equal to 1 if psi0- is set to zero
v_ap = v_ap + 0; % it makes it double instead of logical
v_am = v_am + 0;
m_ap = diag(v_ap);
m_am = diag(v_am);
v_zeros = zeros(N, 1);
if concentrated
     m_Aeq = diag([v_am; v_ap]); % order: psi0-, psi0+
else
     m_beta = [v_zeros v_zeros v_am v_ap]; % N x 4
     m_beta_tr = m_beta'; % 4 x N
     v_beta = m_beta_tr(:); % N4 x 1
     m_Aeq = diag([v_am; v_ap; v_beta; v_zeros]);
end
v_unrestricted_pars = find(sum(m_Aeq, 2) == 0);
m_Aeq(v_unrestricted_pars, :) = [];
v_beq = zeros(size(m_Aeq, 1), 1);

if concentrated
     if isnan(v_initial_point)
          v_psi_ini = zeros(2 * N, 1);
     end

     v_ub = .995 .* ones(2 * N, 1);
     v_lb = -v_ub;

     % iterate until convergence
     CNT = 100;
     v_psi_new = v_psi_ini;
     m_I = eye(T);
     a_theta = zeros(N, K + 3, CNT);
     v_fval = Inf * ones(CNT, 1);
     for cnt = 1:CNT
          v_psi_old = v_psi_new;
          v_psim_old = v_psi_old(1:N, 1);
          v_psip_old = v_psi_old((N + 1):(2 * N), 1);

          m_psim_old = repmat(v_psim_old, 1, T);
          m_psip_old = repmat(v_psip_old, 1, T);
          m_ytilde = m_y - (m_psim_old .* m_ysm) - (m_psip_old .* m_ysp);

          v_a = zeros(N, 1);
          v_b = zeros(N, 1);
          v_c = zeros(N, 1);
          v_d = zeros(N, 1);
          v_e = zeros(N, 1);
          v_f = zeros(N, 1);
          m_beta = zeros(N, K);
          v_sgm = zeros(N, 1);
          for i = 1:N
               v_yi      = m_y     (i, :)';
               v_ysmi    = m_ysm   (i, :)';
               v_yspi    = m_ysp   (i, :)';
               v_ytildei = m_ytilde(i, :)';
               m_Xi = squeeze(a_x(i, :, :));

               v_ones  = m_Xi(:, 1);
               v_lyi   = m_Xi(:, 2);
               v_lysmi = m_Xi(:, 3);
               v_lyspi = m_Xi(:, 4);

               if     v_set(i) == 1; m_Xi = [v_ones v_lyi v_lysmi v_lyspi]; v_K = [1:4]';
               elseif v_set(i) == 2; m_Xi = [v_ones v_lyi v_lysmi]; v_K = [1:3]';
               elseif v_set(i) == 3; m_Xi = [v_ones v_lyi v_lyspi]; v_K = [1 2 4]';
               elseif v_set(i) == 4; m_Xi = [v_ones v_lyi]; v_K = [1 2]';
               end
               invXiXi_Xi = inv(m_Xi' * m_Xi) * m_Xi'; 
               m_beta(i, v_K) = invXiXi_Xi * v_ytildei; 
               m_Mi = m_I - (m_Xi * invXiXi_Xi); 
               v_sgm(i, 1) = sqrt((v_ytildei' * m_Mi * v_ytildei) / T);

               v_a(i, 1) = v_yi'   * m_Mi * v_yi  ;
               v_b(i, 1) = v_ysmi' * m_Mi * v_ysmi;
               v_c(i, 1) = v_yspi' * m_Mi * v_yspi;
               v_d(i, 1) = v_ysmi' * m_Mi * v_yi  ;
               v_e(i, 1) = v_yspi' * m_Mi * v_yi  ;
               v_f(i, 1) = v_ysmi' * m_Mi * v_yspi;
          end

          fn_anonym = @(v_psi)fn_conloglk_N2psi(v_psi, v_a, v_b, v_c, v_d, v_e, v_f, m_Wm, m_Wp, T);

          options.Algorithm = 'interior-point';

          fn_hess_anonym = @(v_psi, lambda)fn_hess_conloglk_N2psi_NKbeta_Nsgm(v_psi, lambda, v_a, v_b, v_c, v_d, v_e, v_f, m_Wm, m_Wp);
          options.HessFcn = fn_hess_anonym;

          [v_psi_new fval exitflag] = fmincon(fn_anonym, v_psi_old, [], [], m_Aeq, v_beq, v_lb, v_ub, [], options);

          % compute the value of the full loglikelihood function
          m_theta_new = [reshape(v_psi_new, N, 2), m_beta, v_sgm];
          a_theta(:, :, cnt) = m_theta_new;

          m_beta_tr = m_beta';
          v_theta_new = [v_psi_new; m_beta_tr(:); v_sgm];
          v_fval(cnt, 1) = fn_uncloglk_N2psi_NKbeta_Nsgm(v_theta_new, m_y, m_ysm, m_ysp, a_x, m_Wm, m_Wp);

          v_diff = v_psi_new - v_psi_old;

          if all(abs(v_diff) < (0.000001 * 2 * N))
               break
          end
     end

     [min_fval ind] = min(v_fval);
     m_theta = a_theta(:, :, ind);
     m_var  = 0 .* m_theta; % no variance returned: here, the concentrated MLE is used only to get a good initial point
else
     if isnan(v_initial_point)
          v_theta_ini = [zeros((K + 2) * N, 1); ones(N, 1)];
     else
          v_theta_ini = v_initial_point;
     end
     
     v_lb = -[0.995 .* ones(2 * N, 1); Inf .* ones(K * N, 1); zeros(N, 1)];
     v_ub =  [0.995 .* ones(2 * N, 1); Inf .* ones((K + 1) * N, 1)];

     fn_anonym = @(v_theta)fn_uncloglk_N2psi_NKbeta_Nsgm(v_theta, m_y, m_ysm, m_ysp, a_x, m_Wm, m_Wp);

     options.Algorithm = 'trust-region-reflective';
     options.Hessian = 'off';
     
     [v_theta fval exitflag output lambda grad hessian] = fmincon(fn_anonym, v_theta_ini, [], [], m_Aeq, v_beq, v_lb, v_ub, [], options);

     v_e_psim = v_theta(1:N, 1); v_e_psip = v_theta((N + 1):(2 * N), 1); v_e_beta = v_theta(((2 * N) + 1):((2 + K) * N), 1); v_e_sgm  = v_theta((end - N + 1):end, 1);
     m_e_beta_tr = reshape(v_e_beta, K, N);
     m_theta = [v_e_psim v_e_psip m_e_beta_tr' v_e_sgm];

     % if 0 numerical hessian, 1 analytic hessian
     if 1
          v_psim = m_theta(:, 1);
          v_psip = m_theta(:, 2);
          m_beta = m_theta(:, 3:(end - 1));
          v_sgm = m_theta(:, end);
          m_beta_tr = m_beta';
          v_theta = [v_psim; v_psip; m_beta_tr(:); v_sgm];
          m_hessian = fn_hess_uncloglk_N2psi_NKbeta_Nsgm(v_theta, NaN, m_y, m_ysm, m_ysp, a_x, m_Wm, m_Wp);
          v_varia = diag(pinv(m_hessian)) ./ T;
          v_v_psim = v_varia(1:N, 1); v_v_psip = v_varia((N + 1):(2 * N), 1); v_v_beta = v_varia(((2 * N) + 1):((2 + K) * N), 1); v_v_sgm  = v_varia((end - N + 1):end, 1);
          m_v_beta_tr = reshape(v_v_beta, K, N);
          m_var = [v_v_psim v_v_psip m_v_beta_tr' v_v_sgm];
     else
          v_varia = diag(pinv(full(hessian))) ./ T;
          v_v_psim = v_varia(1:N, 1); v_v_psip = v_varia((N + 1):(2 * N), 1); v_v_beta = v_varia(((2 * N) + 1):((2 + K) * N), 1); v_v_sgm  = v_varia((end - N + 1):end, 1);
          m_v_beta_tr = reshape(v_v_beta, K, N);
          m_var = [v_v_psim v_v_psip m_v_beta_tr' v_v_sgm];
     end
end

m_eml = m_theta;
m_vml = m_var;

end

% ----------------------------------------------------------------------- %
% -(logL^c(vpsi) / T) 
% v_beta and v_sgm concetrated out
function [ofun v_gradient m_hessian] = fn_conloglk_N2psi(v_psi, v_a, v_b, v_c, v_d, v_e, v_f, m_Wm, m_Wp, T)
N = size(v_a, 1);
v_psim = v_psi(1:N, 1);
v_psip = v_psi((N + 1):end, 1);
m_Psim = diag(v_psim);
m_Psip = diag(v_psip);

m_A = eye(N) - (m_Psim * m_Wm) - (m_Psip * m_Wp);
first_part = -log(abs(det(m_A)));
v_ssr = v_a + (v_psim .* v_psim .* v_b) + (v_psip .* v_psip .* v_c) - (2 * v_psim .* v_d) - (2 * v_psip .* v_e) + (2 * v_psim .* v_psip .* v_f);
secon_part = sum(log(v_ssr / T)) / 2;

ofun = first_part + secon_part;

if nargout > 1
     % gradient
     invA = inv(m_A);
     m_Qm = m_Wm * invA;
     m_Qp = m_Wp * invA;

     v_numm = (v_psim .* v_b) - v_d + (v_psip .* v_f);
     v_nump = (v_psip .* v_c) - v_e + (v_psim .* v_f);
     v_secon_partm = v_numm ./ v_ssr;
     v_secon_partp = v_nump ./ v_ssr;

     v_6f_6psim = diag(m_Qm) + v_secon_partm;
     v_6f_6psip = diag(m_Qp) + v_secon_partp;
     v_gradient = [v_6f_6psim; ...
                   v_6f_6psip];

     if nargout > 2
          v_ssrsq = v_ssr.^2;

          m_secon_partmm = diag((v_b ./ v_ssr) - (2 .* v_numm .* v_numm ./ v_ssrsq));
          m_secon_partmp = diag((v_f ./ v_ssr) - (2 .* v_numm .* v_nump ./ v_ssrsq));
          m_secon_partpp = diag((v_c ./ v_ssr) - (2 .* v_nump .* v_nump ./ v_ssrsq));

          m_Hmm = (m_Qm .* m_Qm') + m_secon_partmm;
          m_Hmp = (m_Qm .* m_Qp') + m_secon_partmp;
          m_Hpp = (m_Qp .* m_Qp') + m_secon_partpp;

          m_hessian = zeros(2 * N);
          m_hessian(1:N, 1:N) = m_Hmm;
          m_hessian(1:N, (N + 1):end) = m_Hmp;
          m_hessian((N + 1):end, 1:N) = m_Hmp';
          m_hessian((N + 1):end, (N + 1):end) = m_Hpp;
     end
end

end

% ----------------------------------------------------------------------- %
% -(logL / T)
% v_theta = (v_psim', v_psip', v_beta_1', ..., v_beta_N', v_sgm')'
function [ofun v_gradient] = fn_uncloglk_N2psi_NKbeta_Nsgm(v_theta, m_y, m_ysm, m_ysp, a_x, m_Wm, m_Wp)
[N T K] = size(a_x);

v_psim = v_theta(1:N, 1);
v_psip = v_theta((N + 1):(2 * N), 1);
v_beta = v_theta(((2 * N) + 1):((2 + K) * N), 1);
v_sgm  = v_theta((end - N + 1):end, 1);

m_beta_tr = reshape(v_beta, K, N);
a_beta = zeros(N, T, K);
for k = 1:K
     v_betak = m_beta_tr(k, :)';
     a_beta(:, :, k) = repmat(v_betak, 1, T);
end

v_sgmsq = v_sgm.^2;
v_sgm3h = v_sgm.^3;

m_psim = repmat(v_psim , 1, T);
m_psip = repmat(v_psip , 1, T);

% compute residuals
m_eps = m_y - (m_psim .* m_ysm) - (m_psip .* m_ysp) - squeeze(sum(a_beta .* a_x, 3));
% standardize
sssr = sum(sum(m_eps .* m_eps, 2) ./ v_sgmsq, 1);

m_psim = diag(v_psim);
m_psip = diag(v_psip);
m_A = eye(N) - (m_psim * m_Wm) - (m_psip * m_Wp);

first_part = -log(abs(det(m_A)));
secon_part = sum(log(v_sgm));
third_part = sssr / T / 2;
ofun = first_part + secon_part + third_part;

if nargin > 1
     m_Qm = m_Wm * inv(m_A);
     m_Qp = m_Wp * inv(m_A);
     v_6f_6psim = diag(m_Qm) - (sum(m_ysm .* m_eps, 2) ./ T ./ v_sgmsq);
     v_6f_6psip = diag(m_Qp) - (sum(m_ysp .* m_eps, 2) ./ T ./ v_sgmsq);

     a_eps = repmat(m_eps, [1, 1, K]);
     m_sgmsq = repmat(v_sgmsq, 1, K);
     mat = [-(squeeze(sum(a_x .* a_eps, 2)) ./ m_sgmsq) ./ T]';
     v_6f_6beta = mat(:);

     v_6f_6sgm = 1 ./ v_sgm - (sum(m_eps .* m_eps, 2) ./ T ./ v_sgm3h);
     v_gradient = [v_6f_6psim; v_6f_6psip; v_6f_6beta; v_6f_6sgm];
end

end
