B
    +¯a  ã               @   s  d dl Zd dlmZ d dlm  mZ d dlZej	 
d¡ d dlm  m  mZ d dlmZ dd„ Zdd„ Zddd„ZedkrdZdZg Zx eeƒD ]Ze 
e e¡¡ q˜W e e¡Ze e¡Zej e¡ZeeddZ eeddZ!ej e¡Z"eeeee e!e"dƒ dS )é    Nz../../..)Úgenerate_random_psd_matrixc             C   sd   |j \}}t | |¡| }t | |¡| | }	d| t |¡ d|| ||    }
||	t |
¡fS )Néÿÿÿÿg      à?)ÚshapeÚjrlinalgÚ
operator_AÚoperator_ATÚSPÚeyeÚnpÚarray)ÚAsÚCÚbÚXÚLambÚvÚtÚnÚrpÚrdÚrc© r   ú8../../JRCVX/jrcvx_sdp/search_directions/AHO_direction.pyÚresidual   s
    
&r   c       
      C   sP   t | ||||||ƒ\}}}	t t t |¡¡t t |¡¡ t t |	¡¡ ¡S )N)r   r
   ÚsqrtÚsumÚsquare)
r   r   r   r   r   r   r   r   r   r   r   r   r   Úresidual_norm   s    r   ç{®Gáz„?ç      à?ç:Œ0âŽyE>c             C   s†  t  t  ||j ¡|
k ¡st‚t  t  ||j ¡|
k ¡s<t‚t  t  ||j ¡|
k ¡sZt‚t  t  | t  | dddg¡ ¡|
k ¡s„t‚|j\}}|dkr t | ¡n|}t	| ||||||ƒ\}}}t 
|¡}t 
|¡}t t |¡|¡}t t |¡|¡}t j |¡}|j| | ||   | }|j | | | }t j ||¡}| ||  }|| ||   }t |¡}t |¡}d}t j |||  ¡\}}x6t  |¡dkrÆ||	9 }t j |||  ¡\}}q’W t j |||  ¡\}}x6t  |¡dkr||	9 }t j |||  ¡\}}qâW t| ||||||ƒ}xLd||  | t| |||||  |||  |||  |ƒkrx||	9 }q.W ||||fS )a	  
	Solve for the linear system for AHO direction

		[0           SVEC(As)^T      0         ]  [ dv         ]      [b - A(x)                             ]
		[SVEC(As)    0               I         ]  [ svec_dX    ]  =   [svec(C - A^T(v) - Lamb)              ]
		[0           I \skro Lamb    I \skro X ]  [ svec_dLamb ]      [svec(1/t I - 0.5*(X*Lamb + Lamb*X))  ]
	with block elimination
	:param As: np.array, of shape [k, n, n]
	:param b:  np.array, of shape [k,]
	:param v:  np.array, the dual variable associated with equality constraint A(X) = b, of shape [k,]
	:param svec_X: np.array, svec of current value of primal variable X
	:param svec_Lamb: np.array, svec of current value of dual variable Lamb associated with inequality constraint X >= 0
	:param alpha: Line search parameter
	:param beta: Line search parameter
	:return:
		dX: Search direction for primal variable X
		dlamb: Search direction for dual variable Lamb
		dv: Search direction for dual variable v
		s: Step size (achieved by line search on the residual norm)

	r   é   é   N)r
   ÚallÚabsÚTÚAssertionErrorÚ	transposer   r   Ú
svec_multir   ZsvecZsymkror   r	   ÚlinalgÚinvÚsolveZsvec_invÚeigÚminr   )r   r   r   r   r   r   r   Úsvec_AsÚalphaÚbetaZsym_tolr   r   r   r   Zsvec_rdZsvec_rcZI_skro_LambZI_skro_XZI_skro_Lamb_invZdv_rhsZdv_lhsÚdvZ
svec_dLambZsvec_dXÚdXÚdLambÚsZevaluesÚ_Únormr   r   r   Úsolve_AHO_system   s@    *




@r7   Ú__main__r!   é
   T)Údefiniter"   )Nr   r   r    )#Únumpyr
   Zscipy.sparseÚsparser   Zscipy.sparse.linalgr)   ZSPLAÚsysÚpathÚappendÚ(JRCVX.jrcvx_sdp.search_directions.linalgÚ	jrcvx_sdpÚsearch_directionsr   ÚJRCVX.utils.random_generater   r   r   r7   Ú__name__Úkr   r   ÚrangeÚiZgenerate_symÚstackr   ÚrandomÚrandnr   r   r   r   r   r   r   r   Ú<module>   s.   
O


