U
    md#                     @   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 ddMdN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ddtduZ:ejdBdBdqddvdwZ;ejdBdBdqedxdrdsdsfdydzZ<ejdBdBdqedxfd{d|Z=ejdBdBdadgedxfd}d~Z>dddZ?dS )    N)
namedtuple)EnumIntEnum   dtype)r   r   g      <g:0yE>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changegh㈵>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rt|	|
|  }nt|	||  }|t|| krt|| }|t|  k r |}||f  S | }q*t|D ]}|| || |	|
|   |	||    }||k rL|}|}|d8 }|dkrt|	|
|  t|	||  krt|	|
|  }nt|	||  }|t|| krt|| }|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|	| }|| rt	}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rt|dkrt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||< || qHqnt
t|D ]}|| }t}t}t}||kr|| |
 nd}t
|d|	 D ]&}|t||  }||k r|}|}q|tkr|t||  qd}t
t|D ]}|| }|| || |||   |||    dkrzq<t|||j|j|} t| |||}!|!jtkrd|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}|rt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 }qt|
|||||	}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rdS d}t|D ]6}|| |kr|| }|| dkrd||< tj||< q|d | }|}d||< d||< d||< ||d< |d ||< |d ||< | ||< d||< |}t|D ]}|||< |||< |d ||< |||d < d||< |||< tj||< || dkrd||< 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rtj}qt| j| j|j|j|}t||| |}|jtkrtjS t||| || |jrt||||| 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rt|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 qd||  }tt| rd q|}|}|d dkrt|	|
||}||kr q|}	|}
|d dkrt||||}||kr qq||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< qd||	  }
tt|
 r^ q|
}|	}|d dkrt||||}||kr qq||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  < qqq|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
	

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