U
    Åmœd#ƒ  ã                   @   s>  d dl Zd dlZd dlmZ d dlmZmZ ejdej	d�Z
ejdej	d�Zejdejd�ZdZdZe ej¡jZe ej¡jZejdejd�Zd	ZG d
d„ deƒZG dd„ deƒZeddddddddddg	ƒZeddddddddd d!g	ƒZed"d#d$d%d&d'd(gƒZed)d*d+d,d-d.gƒZe ¡ d/efd0d1„ƒZejej ej d2œd3�d4d5„ ƒZ!ejej"j#ej"j#d6œd3�d7d8„ ƒZ$ejej#ej#ej#ej#ej#ej#ej%ej d9œd3�d:d;„ ƒZ&ejej#ej ejd<œd3�d=d>„ ƒZ'ejej(ej(ej#ej#ej#ej#ej#ej#ej#ej d?œ
d3�d@dA„ ƒZ)ejdBdCej#ej#ej#dDœdE�dFdG„ ƒZ*e ¡ dHdI„ ƒZ+ejdJej#id3�dKdL„ ƒZ,e ¡ d�dMdN„ƒZ-ejej#ej dOœd3�dPdQ„ ƒZ.e ¡ dRdS„ ƒZ/ejdCdT�dUdV„ ƒZ0ejej#ej#dWœd3�dXdY„ ƒZ1ejdBdJej idZ�d[d\„ ƒZ2ejdBd]�d^d_„ ƒZ3ejdBdBej	ej	d`œdadb�dcdd„ ƒZ4ejdBdBej	ej	d`œdadb�dedf„ ƒZ5ejdBdBdadg�dhdi„ ƒZ6ejdBdBdjej	idadb�dkdl„ ƒZ7ejdBdBdadg�dmdn„ ƒZ8ejdBdBdadg�dodp„ ƒZ9ejdBdBdq�d‚dtdu„ƒZ:ejdBdBdq�dƒdvdw„ƒZ;ejdBdBdq�edxdrdsdsfdydz„ƒZ<ejdBdBdq�edxfd{d|„ƒZ=ejdBdBdadg�edxfd}d~„ƒZ>d„dd€„Z?dS )…é    N)Ú
namedtuple)ÚEnumÚIntEnumé   ©Údtype)r   r   g      ä<g:Œ0âŽyE>éÿÿÿÿc                   @   s   e Zd ZdZdZdZdZdS )ÚProblemStatusr   é   r   é   N)Ú__name__Ú
__module__Ú__qualname__ÚOPTIMALÚMAX_ITER_REACHEDÚ	UNBOUNDEDÚ
INFEASIBLE© r   r   úV/home/sam/Atlas/atlas_env/lib/python3.8/site-packages/pynndescent/optimal_transport.pyr	   4   s   r	   c                   @   s   e Zd ZdZdZdZdS )ÚArcStater   r   r
   N)r   r   r   ÚSTATE_UPPERÚ
STATE_TREEÚSTATE_LOWERr   r   r   r   r   <   s   r   ÚSpanningTreeÚparentÚpredÚthreadÚ
rev_threadÚsucc_numÚ	last_succÚforwardÚstateÚrootÚDiGraphÚn_nodesÚn_arcsÚnÚmÚuse_arc_mixingÚ!num_total_big_subsequence_numbersÚsubsequence_lengthÚnum_big_subsequencesÚmixing_coeffÚNodeArcDataÚcostÚsupplyÚflowÚpiÚsourceÚtargetÚLeavingArcDataÚu_inÚu_outÚv_inÚdeltaÚchangegñhãˆµøä>c                 C   s$   t  | | ¡}|||t  |¡  kS ©N)ÚnpÚabs)ÚaÚbZrtolZatolÚdiffr   r   r   Úisclosen   s    r@   )r=   Úe)Úlocalsc                 C   s~  d}| }|j }|j}	|j}
|j}t||ƒD ]Ú}|| || |	|
|   |	||    }||k rf|}|}|d8 }|dkr*t |	|
|  ¡t |	||  ¡kr®t |	|
|  ¡}nt |	||  ¡}|t || ¡kràt || ¡}|t|  k �r |}||f  S | }q*t|ƒD ]ä}|| || |	|
|   |	||    }||k �rL|}|}|d8 }|dk�rt |	|
|  ¡t |	||  ¡k�r˜t |	|
|  ¡}nt |	||  ¡}|t || ¡k�rÌt || ¡}|t|  k �rì|}||f  S | }�qt |	|
|  ¡t |	||  ¡k�r.t |	|
|  ¡}nt |	||  ¡}|t || ¡k�rbt || ¡}|t|  k�rvdS ||fS )Nr   r
   )r   r   )r.   r1   r2   r3   Úranger;   ÚfabsÚEPSILON)Úpivot_block_sizeÚpivot_next_arcÚsearch_arc_numZstate_vectorÚnode_arc_dataÚin_arcÚminZcntr.   r1   r2   r3   rA   Úcr=   r   r   r   Úfind_entering_arcv   s\    	($(

&&rM   )ÚuÚvc                 C   sD   | | }|| }||kr<|| || k r2|| }q|| }q|}|S r:   r   )r2   r3   r   r   rJ   rN   rO   Újoinr   r   r   Úfind_join_nodeÈ   s    

rQ   )rN   r5   r6   r7   ÚfirstÚsecondÚresultrJ   c                 C   s&  |j }|j}|j}|j}|j}|j}	|j}
d}|| tjkrN|| }|| }n|| }|| }t	}d}|}|| kr®|	| }|| rŒ|| }nt	}||k r¤|}|}d}|
| }qj|}|| krö|	| }|| rÐt	}n|| }||krì|}|}d}|
| }q²|dk�r
|}|}n|}|}t
|||||dkƒS )Nr   r   r
   r   )r2   r3   r0   r!   r    r   r   r   r   ÚINFINITYr4   )rP   rJ   rI   Úspanning_treer2   r3   r0   r!   r    r   r   r6   rR   rS   r8   rT   rN   rA   Údr5   r7   r   r   r   Úfind_leaving_arcÛ   sV    




rX   )rN   rJ   Úvalc                 C   sF  |j }|j}|j}|j}|j}	|j}
|j}|jdkrê|| |j }||  |7  < || }|| krž|| r€||	|   |8  < n||	|   |7  < |
| }qZ|| }|| krê|| rÌ||	|   |7  < n||	|   |8  < |
| }q¦|j�r4t	j
||< ||	|j  dk�r"t	j||	|j < nt	j||	|j < n||  ||< d S )Nr   )r2   r3   r0   r!   r   r   r    r8   r9   r   r   r6   r   r   )rP   Úleaving_arc_datarI   rV   rJ   r2   r3   r0   r!   r   r   r    rY   rN   r   r   r   Úupdate_flow,  s8    



r[   )
rN   Úwr5   r6   r7   ÚrightÚstemÚnew_stemÚpar_stemrJ   c                  C   s2  | j }| j}| j}| j}| j}	| j}
| j}|j}|j}|j	}|| }|| }|	| }|| }|	| }|| }||kr‚||	|  }n|| }| ||< }g }| 
|¡ |}||k�r"|| }|||< | 
|¡ || }|||< |||< |||< |}|}|	| |	| k�r|| }n|	| }|| }q¨|||< |||< |||< ||	|< ||k�r\|||< |||< tt|ƒƒD ]}|| }|||| < �qhd}|	| }|}||k�rì|| }|| ||< |
|  |
|< ||| ||  7 }|||< ||	|< |}�q”|||< ||| k|
|< |||< d}d}|	| |k�r(|}n|}|}||k�r`|	| |k�r`|	| |	|< || }�q0||k�r¦||k�r¦|}||k�rÚ|	| |k�rÚ||	|< || }�qxn4|}||k�rÚ|	| |k�rÚ|	| |	|< || }�qª|}||k�r||  |7  < || }�qÞ|}||k�r.||  |8  < || }�qd S )Nr   r   )r   r   r   r   r   r    r   r6   r5   r7   ÚappendrC   Úlen) rV   rZ   rP   rJ   r2   r   r   r   r   r   r    r   r6   r5   r7   Zold_rev_threadZold_succ_numZold_last_succZv_outrN   r]   Úlastr^   Z
dirty_revsr`   r_   r\   ÚiZtmp_scZtmp_lsZup_limit_inZup_limit_outr   r   r   Úupdate_spanning_tree]  s®    








re   TÚalways)rN   r5   r7   )ÚfastmathÚinlinerB   c                 C   sœ   |j }|j}|j}|j}| j}| j}	|| rJ||	 ||  |||   }
n||	 ||  |||   }
|||  }|}||kr˜||  |
7  < || }qvd S r:   )r   r   r    r   r5   r7   )rZ   r1   r.   rV   r   r   r    r   r5   r7   ÚsigmaÚendrN   r   r   r   Úupdate_potentialõ  s    rk   c                 C   sj   |j |  d }|jrb||jkd@ }||j| 8 }|j| }|| |j|  }|| |j }|| S |S d S ©Nr
   )r%   r(   r)   r*   r+   r,   )ÚarcÚgraphÚkZsmallvZsubsequence_length2Zsubsequence_numZsubsequence_offsetr   r   r   Úarc_id  s    
þrp   rd   c           "      C   sö  |j }|j}|j}|j}|j}| j}| j}	| j}
| j}|j	}d}g }g }t
|
ƒD ]B}||
| d  }|dkr€||7 }| |¡ qP|dk rP| |¡ qPg }t|ƒdk�ršt|ƒdk�rštj|
tjd�}|d }|d }g }d||< | |¡ t|ƒdk�r,|d }|d }| d¡ ||k�r �q,||k�r6|| |
 nd}t
|d|	 ƒD ]L}||	 }|| �rb�qHt|| ƒ}t|k�rH| |¡ d||< | |¡ �qHqên’t
t|ƒƒD ]„}|| }t}t}t}||k�rÔ|| |
 nd}t
|d|	 ƒD ]&}|t|| ƒ }||k �ræ|}|}�qæ|tk�r¦| t|| ƒ¡ �q¦d}t
t|ƒƒD ]°}|| }|| || |||   |||    dk�rz�q<t|||j|j|ƒ} t| |||ƒ}!|!jtk�r´d|f  S t| |!|||ƒ |!j�r<t||!| ||ƒ t|!|||ƒ �q<d|fS )Nr   r
   r   Tr   F)r.   r1   r2   r3   r/   r&   r'   r$   r%   r!   rC   ra   rb   r;   ÚzerosÚbool_Úpoprp   rU   ÚMAXÚINVALIDrQ   r   r   rX   r8   r[   r9   re   rk   )"rn   rI   rV   r.   r1   r2   r3   r/   Zn1Zn2r$   r%   r!   ÚtotalZsupply_nodesZdemand_nodesrN   ÚcurrZ
arc_vectorZreachedÚsÚtÚstackrO   Z	first_arcr=   Újrd   rL   Zmin_costZmin_arcrJ   rP   rZ   r   r   r   Úconstruct_initial_pivots'  s     








&ÿÿ    ÿr|   c           !   
   C   s\  | | }| | }|d }|d|  }|}t j|t jd�}t j|t jd�}	t j|t jd�}
t j|t jd�}t j|t jd�}t j|t jd�}t j|t jd�}t j|t jd�}t j|t jd�}t j|t jd�}t j|t jd�}t j|t jd�}t j|t jd�}t j|t jd�}|�r°tt  t  	|¡¡dƒ}|}|| d }|| }|| }d}d}t
|d ddƒD ]P}|||  d ||< ||| |   d |	|< ||7 }||k�r\|d7 }|}�q\n`d}d}d}d}d}t
|d ddƒD ]:}|||  d ||< ||| |   d |	|< |d7 }�qÔt|
|||||	ƒ}t|||||||||ƒ	}t||| ||||||ƒ	} ||| fS )Nr
   r   r   é
   r   r   )r;   rq   Úuint16ÚonesÚfloat64Úint32rr   Zint8ÚmaxÚsqrtrC   r-   r   r#   )!r&   r'   r(   r$   r%   Zall_node_numZmax_arc_numr"   r2   r3   r.   r/   r0   r1   r   r   r    r   r   r   r   r!   ro   r,   r*   r+   r)   rd   r{   r=   rI   rV   rn   r   r   r   Úallocate_graph_structuresŠ  s†    

        ÿ÷r„   )rN   rA   c                 C   s  | j }| j}|j}|j}|j}|j}|j}	|j}
|j}|j	}|j
}|j}|j}|j}|j}|j}|dkrldS d}t|ƒD ]}||| 7 }qxt |¡tkrœdS d}t|ƒD ]6}|| |krÀ|| }|| dkrÔd||< tj||< q¨|d | }|}d||< d||< d||< ||d< |d ||< |d ||< | ||< d||< |}t|ƒD ]Ê}|||< |||< |d ||< |||d < d||< |||< tj||< || dk�rÐd||< d||< ||	|< ||
|< || ||< d||< n6d||< |||< ||	|< ||
|< ||  ||< |||< |d7 }�qFdS )Nr   Fç        r
   r   T)r$   r%   r.   r/   r0   r1   r2   r3   r   r   r   r   r   r   r    r!   rC   r;   rD   ÚNET_SUPPLY_ERROR_TOLERANCEr   r   r   )rn   rI   rV   r$   r%   r.   r/   r0   r1   r2   r3   r   r   r   r   r   r   r    r!   Z
net_supplyrd   Zartificial_costr"   rA   rN   r   r   r   Úinitialize_graph_structuresØ  s€    


r‡   c                 C   sR   t |jƒD ]B}||jk r0| | ||j| d < q
|||j  ||j| d < q
d S rl   )rC   r$   r&   )Zleft_node_supplyZright_node_supplyrn   r/   r&   r   r   r   Úinitialize_supply0  s    
rˆ   )rh   c                 C   s   ||t | |ƒ< d S r:   )rp   )rm   Zcost_valr.   rn   r   r   r   Úset_cost9  s    r‰   )rd   r{   c                 C   sP   t | jd ƒD ]<}t | jd ƒD ](}t|| jd  | | ||f ||ƒ q qd S ©Nr   r
   )rC   Úshaper‰   )Zcost_matrixrn   r.   rd   r{   r   r   r   Úinitialize_cost>  s    rŒ   )rg   rB   c                 C   s0   d}t | jd ƒD ]}|| | ||  7 }q|S ©Nr…   r   )rC   r‹   )r0   r.   rL   rd   r   r   r   Ú
total_costE  s    rŽ   )Znogilc                 C   sÖ  t t t |j¡¡dƒ}|j}tj}t|| |ƒ\}}|s>tjS d}	t	|d||j
| |ƒ\}}
|dk�r|	d7 }	|dkr†|	|kr†tj}�qt| j| j|j|j|ƒ}t||| |ƒ}|jtkr¼tjS t||| ||ƒ |jröt||||| jƒ t|| j| j|ƒ t	||
||j
| |ƒ\}}
qZ| j}| j}|tjk�rxt|j|j|j ƒD ]:}|| dk�r<t || ¡tk�rltj  S d||< �q<t  }t|jƒD ]}|| |k�rˆ|| }�qˆ|dk�rÒt|jƒD ]}||  |8  < �qº|S )Nr}   r   r
   )!r‚   r;   r�   rƒ   r%   r	   r   r|   r   rM   r!   r   rQ   r2   r3   r   r   rX   r8   rt   r[   r9   re   rk   r1   r.   r0   rC   r$   r<   rE   r   rU   )rI   rV   rn   Úmax_iterrF   rH   Zsolution_statusZboundedrJ   Ziter_numberrG   rP   rZ   r0   r1   rA   Zmax_potrd   r   r   r   Únetwork_simplex_coreM  sŠ         ÿ
û
    ÿ   ÿú
	

r�   )r?   rT   F)rg   ÚparallelrB   Úcachec                 C   sP   | | }d}t  |jd ¡D ](}|| || ||   }||| 7 }qt |¡S r�   )ÚnumbaÚpranger‹   r;   rƒ   )rN   ÚKrO   ÚyÚuKrT   rd   r?   r   r   r   Úright_marginal_error¦  s    r˜   c           	      C   sr   |j |  }d}t |jd ¡D ]H}t|jd ƒD ]4}|||f |||f |||f   }||| 7 }q0qt |¡S )Nr…   r   r
   )ÚTr“   r”   r‹   rC   r;   rƒ   )	rN   r•   rO   r–   r—   rT   rd   r{   r?   r   r   r   Úright_marginal_error_batchµ  s    
$rš   )rg   r‘   r’   c                 C   sd   | j d }| j d }t | ¡}t |¡D ]6}t|ƒD ](}|| | ||f  ||  |||f< q4q(|S rŠ   )r‹   r;   Ú
empty_liker“   r”   rC   )r•   rN   rO   Úi_dimÚj_dimrT   rd   r{   r   r   r   Útransport_planÅ  s    


(rž   rT   c           
      C   sz   | j d }|j d }d}t |¡D ]J}t|ƒD ]<}| | ||  }	|t t |	|| ||   ¡|	 ¡7 }q.q"|||  S )Nr   r…   )r‹   r“   r”   rC   r;   Úfloat32r<   )
Zold_uÚold_vZnew_uÚnew_vrœ   r�   rT   rd   r{   Zold_uvr   r   r   Úrelative_change_in_planÑ  s    

,r¢   c                 C   sv   | j d }| j d }t | ¡}t |¡D ]H}|| dkrFd||  }nt}t|ƒD ]}|| ||f  |||f< qRq(|S )Nr   r
   r…   ç      ð?)r‹   r;   r›   r“   r”   rU   rC   )r•   Úxrœ   r�   rT   rd   Zx_i_inverser{   r   r   r   Úprecompute_K_primeÞ  s    


r¥   c                 C   sd   | j d }| j d }t | ¡}t |¡D ]6}t|ƒD ](}| ||f | }t | ¡|||f< q4q(|S rŠ   )r‹   r;   r›   r“   r”   rC   Úexp)r.   Úregularizationrœ   r�   rT   rd   r{   Zscaled_costr   r   r   ÚK_from_costî  s    


r¨   )rg   r’   éè  ç•Ö&è.>c                 C   sÊ   t || ƒ}|}	|}
t|ƒD ]¦}||j|  }t t |¡ ¡rB qÂd||  }t t |¡ ¡rd qÂ|}|}|d dkršt|	|
||ƒ}||kr’ qÂ|}	|}
|d dkrt||||ƒ}||kr qÂq||fS )Nr£   é   r   r}   )r¥   rC   r™   r;   ÚanyÚisfiniter¢   r˜   )r¤   r–   rN   rO   r•   r�   Úerror_toleranceÚchange_toleranceÚK_primeZprev_uZprev_vÚ	iterationÚnext_vÚnext_uZrelative_changeÚerrr   r   r   Úsinkhorn_iterationsû  s.    
rµ   c                 C   s–   t || ƒ}t|ƒD ]z}|j|j|  }	t t |	¡ ¡r< qŽd||	  }
t t |
¡ ¡r^ qŽ|
}|	}|d dkrt||||ƒ}||kr qŽq||fS )Nr£   r}   r   )r¥   rC   r™   r;   r¬   r­   rš   )r¤   r–   rN   rO   r•   r�   r®   r°   r±   r²   r³   r´   r   r   r   Úsinkhorn_iterations_batch%  s    
r¶   r£   c              
   C   sr   | j d }|j d }tj|d| |jd�}	tj|d| |jd�}
t||ƒ}t| ||	|
||||d�\}	}
t||	|
ƒS )Nr   r£   r   )r�   r®   r¯   )r‹   r;   Úfullr   r¨   rµ   rž   )r¤   r–   r.   r§   r�   r®   r¯   Údim_xÚdim_yrN   rO   r•   r   r   r   Úsinkhorn_transport_planA  s     



ø
rº   c           
      C   sd   t | |||d�}|jd }|jd }d}t|ƒD ].}t|ƒD ] }	||||	f |||	f  7 }q<q0|S )N)r.   r§   r   r
   r…   )rº   r‹   rC   )
r¤   r–   r.   r§   rž   Zdim_iZdim_jrT   rd   r{   r   r   r   Úsinkhorn_distance_  s       ÿ

 r»   c              
   C   sü   | j d }|j d }|j d }tj||fd| |jd�}tj||fd| |jd�}t||ƒ}	t| ||||	ƒ\}}|	j d }
|	j d }t |¡}t|
ƒD ]`}t|ƒD ]R}|	||f |||f  }t|ƒD ],}||  |||f | |||f  7  < qÆq¢q–|S )Nr   r
   r£   r   )r‹   r;   r·   r   r¨   r¶   rq   rC   )r¤   r–   r.   r§   r¸   r¹   Z
batch_sizerN   rO   r•   rœ   r�   rT   rd   r{   ZK_times_costÚbatchr   r   r   Úsinkhorn_distance_batchn  s,    



û


.r½   c                    s@   t ˆ ˆƒ}|jd ‰|jd ‰tjdd�‡ ‡‡‡fdd„ƒ}|S )Nr   r
   T)rg   c           	         s–   t jˆdˆ ˆ jd�}t jˆdˆ ˆ jd�}tˆ ˆƒ}t| ||||ƒ\}}t|||ƒ}d}tˆƒD ].}tˆƒD ] }||||f ˆ ||f  7 }qnqb|S )Nr£   r   r…   )r;   r·   r   r¨   rµ   rž   rC   )	r¤   r–   rN   rO   r•   Zcurrent_planrT   rd   r{   ©r.   r¸   r¹   r§   r   r   Úclosure“  s     
û z2make_fixed_cost_sinkhorn_distance.<locals>.closure)r¨   r‹   r“   Únjit)r.   r§   r•   r¿   r   r¾   r   Ú!make_fixed_cost_sinkhorn_distance�  s    



rÁ   )T)r©   rª   rª   )r©   rª   )r£   )@Únumpyr;   r“   Úcollectionsr   Úenumr   r   ÚeyerŸ   Z_mock_identityr   Z
_mock_onesrq   r€   Z_dummy_costrE   r†   Zfinfor‚   rU   rt   Z
dummy_costru   r	   r   r   r#   r-   r4   rÀ   r@   Zuint32rM   Útypesr~   rQ   Zuint8rX   r[   r�   re   rk   rp   r|   r„   r‡   rˆ   r‰   rŒ   rŽ   r�   r˜   rš   rž   r¢   r¥   r¨   rµ   r¶   rº   r»   r½   rÁ   r   r   r   r   Ú<module>   s4  ÷þ÷þúþ ÿ
Q
øÿ
E
0öÿ
 ý


bM
W






Xü
	ü





     ÿ)ù