
function V = nonconvex_orth2d_quad_L0(H,B,lambda,Z,myeps)
HandleObj = @(V)0.5*vec(V)'*H*vec(V) + mdot(V,B)+ lambda* nnz3(V*Z,myeps) ;
% min_{V} HandleObj(V), s.t. V'V = I

% We delete the zero column of Z
ZZ = abs(Z); norm1 = sum(ZZ,1);
Index = (norm1>myeps);
Z = Z(:, Index);

[V1,f1] = nonconvex_orth2d_quad_L0_R(H,B,lambda,Z,myeps);
[V2,f2] = nonconvex_orth2d_quad_L0_F(H,B,lambda,Z,myeps);
% f1 = HandleObj(V1);
% f2 = HandleObj(V2);

if(f1<f2)
    V = V1;
else
    V = V2;
end





function[V,fmin] = nonconvex_orth2d_quad_L0_R(Q,B,lambda,Z,myeps)
% 0.5*vec(V)'*H*vec(V) + mdot(V,B)
% V \in rotator(t)
% f = 0.5*vec(V)'*Q*vec(V) + mdot(V,B) + lambda ||VX||_1, s,t, V'V = I
% V = [c s
%     -s c];

cof_r = Z(1,:)';
cof_s = Z(2,:)';
cof_p = Z(2,:)';
cof_u = -Z(1,:)';
cof_x = [cof_r;cof_p];
cof_y = [cof_s;cof_u];

cof_a = B(1,1) + B(2,2);
cof_b = B(1,2) - B(2,1);
cof_c =  0.5*Q(1,1)+ Q(1,4) + 0.5*Q(4,4);
cof_d =  - Q(1,2) + Q(1,3) -Q(2,4) + Q(3,4);
cof_e =  0.5*Q(2,2) - Q(2,3) + 0.5*Q(3,3);


%  [c,s,fmin] = nonconvex_quadratic_trigonometry_L0_v0(cof_a,cof_b,cof_c,cof_d,cof_e,lambda,cof_x,cof_y);
   [c,s,fmin] = nonconvex_quadratic_trigonometry_L0(cof_a,cof_b,cof_c,cof_d,cof_e,lambda,cof_x,cof_y,myeps);
%  [c,s,fmin] = nonconvex_quadratic_trigonometry_L0_cc(cof_a,cof_b,cof_c,cof_d,cof_e,lambda,cof_x,cof_y,myeps,length(cof_x));

V = [c s;
    -s c];
% [c1,s1] = nonconvex_quadratic_trigonometry_L0_v0(cof_a,cof_b,cof_c,cof_d,cof_e,lambda,cof_x,cof_y);
% if(abs(c1 - c) + abs(s1-s) > 1e-5)
%     ddd
% end

function [V,fmin] = nonconvex_orth2d_quad_L0_F(H,B,lambda,Z,myeps)
% 0.5*vec(V)'*H*vec(V) + mdot(V,B) + lambda * ||VX||_0
% V \in V = [ -c s;
%             s c];
cof_r  = -Z(1,:)';
cof_s  = Z(2,:)';
cof_p  = Z(2,:)';
cof_u  = Z(1,:)';
cof_x  = [cof_r;cof_p];
cof_y  = [cof_s;cof_u];
cof_c = 0.5*H(1,1) - 0.5*H(1,4) - 0.5*H(4,1) +0.5*H(4,4);
cof_e = 0.5*H(2,2)+ 0.5*H(2,3)+ 0.5*H(3,2) + 0.5*H(3,3);
cof_d = - 0.5*H(1,2) - 0.5*H(1,3) - 0.5*H(2,1) +  0.5*H(2,4) - 0.5*H(3,1) + 0.5*H(3,4) +  0.5*H(4,2)+ 0.5*H(4,3);
cof_a  = -B(1)+ B(4);
cof_b  =  B(2) + B(3);
 [c,s,fmin] = nonconvex_quadratic_trigonometry_L0(cof_a,cof_b,cof_c,cof_d,cof_e,lambda,cof_x,cof_y,myeps);
% [c,s,fmin] = nonconvex_quadratic_trigonometry_L0_cc(cof_a,cof_b,cof_c,cof_d,cof_e,lambda,cof_x,cof_y,length(cof_x));

% [c1,s1,fmin2] = nonconvex_quadratic_trigonometry_L0_v0(cof_a,cof_b,cof_c,cof_d,cof_e,lambda,cof_x,cof_y);
% if(abs(fmin - fmin2) > 1e-5)
%     fmin
%     fmin2
%     c1
%     s
%     s1
%     ddd
% end



V = [-c s;
    s c];



function [c,s,fmin] = nonconvex_quadratic_trigonometry_L0(A,B,C,D,E,lambda,x,y,myeps)
% min_{c,s}  A*c +B*s  + C*c*c  + D*c*s +  E*s*s  + lambda ||c*x+s*y||_0
eps = 2.2204e-16;

HandleObj2 = @(c,s)A*c + B*s  + C*c^2 + D*s*c + E*s*s + lambda*nnz3(c*x+s*y,myeps);

cos_list=[0;0;1;-1]; sin_list = [1;-1;0;0];

[critical_points] = get_all_critical_points(A,B,C,D,E);

% case 1
for iii = 1:length(critical_points)
    t_case_1 = critical_points(iii);
    chi = 1/sqrt(1+t_case_1*t_case_1);
    cos_theta1 = chi;
    sin_theta1 = t_case_1*chi;
    cos_list = [cos_list;cos_theta1];
    sin_list = [sin_list;sin_theta1];
end
% case 2
for iii=1:length(critical_points)
    t_case_2 = critical_points(iii);
    chi = 1/sqrt(1+t_case_2*t_case_2);
    cos_theta2 = -chi;
    sin_theta2 = -t_case_2*chi;
    cos_list = [cos_list;cos_theta2];
    sin_list = [sin_list;sin_theta2];
end



for iii=1:length(x)
    if(abs(y(iii))>eps)
        % case 1
        t_case_1 = x(iii) ./ y(iii);
        chi = 1/sqrt(1+t_case_1*t_case_1);
        cos_theta1 = chi;
        sin_theta1 = t_case_1*chi;
        cos_list = [cos_list;cos_theta1];
        sin_list = [sin_list;sin_theta1];
        
        t_case_1 = -t_case_1;
        chi = 1/sqrt(1+t_case_1*t_case_1);
        cos_theta1 = chi;
        sin_theta1 = t_case_1*chi;
        cos_list = [cos_list;cos_theta1];
        sin_list = [sin_list;sin_theta1];
        
        % case 2
        t_case_2 = x(iii) ./ y(iii);
        chi = 1/sqrt(1+t_case_2*t_case_2);
        cos_theta2 = -chi;
        sin_theta2 = -t_case_2*chi;
        cos_list = [cos_list;cos_theta2];
        sin_list = [sin_list;sin_theta2];
        
        t_case_2 = -t_case_2;
        chi = 1/sqrt(1+t_case_2*t_case_2);
        cos_theta2 = -chi;
        sin_theta2 = -t_case_2*chi;
        cos_list = [cos_list;cos_theta2];
        sin_list = [sin_list;sin_theta2];
        
    end
end






for i=1:length(cos_list)
    fs(i) = HandleObj2(cos_list(i),sin_list(i));
end

[fmin,j] = min(fs);
c = cos_list(j);
s = sin_list(j);


function [x] = get_all_critical_points(a,b,c,d,e)
% This program get the critical points of the following unconstrainted problem:
%            a  +  bt                 c - e + d*t
% min_t  -------------------   +   -----------------
%            sqrt(1+t*t)               1+t*t

w = c -e;
c0 = d*d - b*b;
c1 = 2*b*a - 4*d*w;
c2 = 4*w*w - 2*d*d -a*a - b*b;
c3 = 4*w*d + 2*a*b;
c4 = d*d - a*a;

% grad = @(t)c0 + c1*t +c2*t*t + c3 * t*t*t + c4*t*t*t*t ;
ts = roots([c4;c3;c2;c1;c0]);
x = real(ts);
if(isempty(x))
    x = 0;
end

