U
    f»{jxÒ  ã                   @   sÌ  d Z ddlmZ ddlZddlZddlmZ ddlm	Z	 ddl
mZ ddlmZ dd	lmZmZ dd
lmZmZmZmZmZ ddlmZmZ ddlmZ ddlmZ ddlm Z m!Z!m"Z" ddl#m$Z$m%Z% ddl&m'Z' ddl(m)Z) ddl*m+Z+m,Z,m-Z-m.Z. ddl/m0Z0m1Z1m2Z2m3Z3m4Z4m5Z5m6Z6m7Z7 ddl8m9Z9m:Z:m;Z;m<Z<m=Z=m>Z>m?Z?m@Z@mAZAmBZB dd„ ZCdd„ ZDdd„ ZEe9dadd„ƒZFe9dbddœdd „ƒZGe9dcd"d#„ƒZHe9ddd%d&„ƒZIded'd(„ZJe)ƒ d)d*„ ƒZKd+d,„ ZLd-d.„ ZMe)ƒ d/d0„ ƒZNd1d2„ ZOdfd4d5„ZPe9dgd6d7„ƒZQd8d9„ ZRe)ƒ d:d;„ ƒZSd<d=„ ZTd>d?„ ZUdhd@dA„ZVe)ƒ dBdC„ ƒZWe)ƒ dDdE„ ƒZXdFdG„ ZYe9didJdK„ƒZZdLdM„ Z[e)ƒ dNdO„ ƒZ\e9djdRdS„ƒZ]e9dkdTdU„ƒZ^dldVdW„Z_e9dmdYdZ„ƒZ`d[d\„ Zae9dnd]d^„ƒZbdod_d`„ZcdS )pz7Functions for fitting head positions with (c)HPI coils.é    )ÚpartialNé   )Úfind_events)ÚBaseRaw)ÚKIT)ÚRawKIT)Ú_simplify_infoÚInfo)Ú
pick_typesÚpick_channelsÚpick_channels_regexpÚ	pick_infoÚ_picks_to_idx)Ú
ProjectionÚ
setup_proj)ÚFIFF)Ú_make_ctf_coord_trans_set)Ú_magnetic_dipole_field_vecÚ_create_meg_coilsÚ_concatenate_coils)Úmake_ad_hoc_covÚcompute_whitener)Ú_make_guesses)Újit)Ú
_sss_basisÚ_prep_mf_coilsÚ_regularize_outÚ_get_mf_picks_fix_mags)Úapply_transÚinvert_transformÚ_angle_between_quatsÚquat_to_rotÚrot_to_quatÚ_fit_matched_pointsÚ_quat_to_affineÚals_ras_trans)
ÚverboseÚloggerÚuse_log_levelÚ_check_fnameÚwarnÚ_validate_typeÚProgressBarÚ_check_optionÚ_plÚ_on_missingc                 C   s@   t | ddd� tj| dd�}d|_t |¡ ¡ r<td|  ƒ‚|S )aÁ  Read MaxFilter-formatted head position parameters.

    Parameters
    ----------
    fname : str
        The filename to read. This can be produced by e.g.,
        ``maxfilter -headpos <name>.pos``.

    Returns
    -------
    pos : array, shape (N, 10)
        The position and quaternion parameters from cHPI fitting.

    See Also
    --------
    write_head_pos
    head_pos_to_trans_rot_t

    Notes
    -----
    .. versionadded:: 0.12
    TÚread)Z
must_existÚ	overwriter   )Zskiprows)éÿÿÿÿé
   z,positions could not be read properly from %s)r)   ÚnpZloadtxtÚshapeÚisnanÚanyÚRuntimeError)ÚfnameÚdata© r;   úG/var/www/html/bwave/python/venv/lib/python3.8/site-packages/mne/chpi.pyÚread_head_pos<   s    ÿr=   c              	   C   s¤   t | dd� t |tj¡}|jdks2|jd dkr:tdƒ‚t| dƒ�V}| d 	d	¡¡ |D ]:}d
gdgd  }| dd 
|¡ d t|ƒ  	d	¡¡ qZW 5 Q R X dS )a`  Write MaxFilter-formatted head position parameters.

    Parameters
    ----------
    fname : str
        The filename to write.
    pos : array, shape (N, 10)
        The position and quaternion parameters from cHPI fitting.

    See Also
    --------
    read_head_pos
    head_pos_to_trans_rot_t

    Notes
    -----
    .. versionadded:: 0.12
    T)r1   é   r   r3   z'pos must be a 2D array of shape (N, 10)Úwbz] Time       q1       q2       q3       q4       q5       q6       g-value  error    velocity
ÚASCIIz% 9.3fz% 8.5fé	   ú Ú
N)r)   r4   ÚarrayÚfloat64Úndimr5   Ú
ValueErrorÚopenÚwriteÚencodeÚjoinÚtuple)r9   ÚposÚfidÚpZfmtsr;   r;   r<   Úwrite_head_pos\   s    ÿÿÿrP   c                 C   s>   | d   ¡ }t| ddd…f ƒ}| ddd…f   ¡ }|||fS )aê  Convert Maxfilter-formatted head position quaternions.

    Parameters
    ----------
    quats : ndarray, shape (N, 10)
        MaxFilter-formatted position and quaternion parameters.

    Returns
    -------
    translation : ndarray, shape (N, 3)
        Translations at each time point.
    rotation : ndarray, shape (N, 3, 3)
        Rotations at each time point.
    t : ndarray, shape (N,)
        The time points.

    See Also
    --------
    read_head_pos
    write_head_pos
    ).r   .r   é   é   )Úcopyr!   )ÚquatsÚtÚrotationÚtranslationr;   r;   r<   Úhead_pos_to_trans_rot_t|   s    rX   c              
      s*  t ˆ jd dƒ}t|ƒdkr$tdƒ‚t t|‡ fdd„d�¡}|dd	d
ddddddg	 }~tdtˆ jƒƒ}ˆ ||f d }t	ddƒ}|d }~dg}| 
t tjtj|dd�dd�¡d d ¡ |dd…|f j t|ƒdd¡}	t||	ƒ}	t |	jdd… ¡}
t |	j¡}ˆ j| ˆ j }t|	|
||d�S )a0  Extract cHPI locations from CTF data.

    Parameters
    ----------
    raw : instance of Raw
        Raw data with CTF cHPI information.
    %(verbose)s

    Returns
    -------
    %(chpi_locs)s

    Notes
    -----
    CTF continuous head monitoring stores the x,y,z location (m) of each chpi
    coil as separate channels in the dataset:

    - ``HLC001[123]\\*`` - nasion
    - ``HLC002[123]\\*`` - lpa
    - ``HLC003[123]\\*`` - rpa

    This extracts these positions for use with
    :func:`~mne.chpi.compute_head_pos`.

    .. versionadded:: 0.20
    Úch_nameszHLC00[123][123].*rA   z"Could not find all 9 cHPI channelsc                    s   ˆ j d |  S )NrY   ©Úinfo)Úk©Úrawr;   r<   Ú<lambda>½   ó    z'extract_chpi_locs_ctf.<locals>.<lambda>©Úkeyé   rQ   é   r   r   r>   é   rR   é   NZt_ctf_dev_dev©Zaxis©ÚrrsÚgofsÚtimesÚmoments)r   r[   Úlenr8   r4   rD   ÚsortedÚslicerk   r   ÚextendÚwhereÚallÚdiffÚTÚreshaper   Úonesr5   ÚzerosZ_first_timeÚdict)r^   r&   Z	hpi_picksZsorted_picksÚtime_slÚ	chpi_dataZ	tmp_transZctf_dev_dev_tÚindicesri   rj   rl   rk   r;   r]   r<   Úextract_chpi_locs_ctf˜   s*    
ÿ
, 
r|   úMISC 064©r&   c             
      s’  t ˆ tfdƒ ‡ fdd„tˆ jdddd�D ƒ}t |tdƒ td||ƒ ˆ j |¡}tˆ ˆ j| ddd	�d
d
…df }tˆ ˆ j| ddd	�d
d
…df }d}t	|ƒdks²t	|ƒdkr¸d}n8|d |d krÔ|d
d… }|j
|j
ksì||k  ¡ sðd}|�rtd|› �ƒ‚|| dˆ jd   }~~|dd
… }d}	ˆ jd d tj }
t ddg¡}|j|
d k�srt|j|
d fƒ‚tƒ }ˆ jD ]L}t|dƒ�6}| |
d ¡ | tj|||
d d� d|	¡¡ W 5 Q R X �q~t |¡}d}t	|ƒt	|ƒk �rüdt	|ƒ› d�}t dt	|ƒ› dtt	|ƒƒ› |› �¡ |d
t	|ƒ… }|d }|d
d
…d
d
…d
d…f |d
d
…d
d
…df  }}tt|ƒ}t  |j!¡}t"||||d �S )!aY  Extract cHPI locations from KIT data.

    Parameters
    ----------
    raw : instance of RawKIT
        Raw data with KIT cHPI information.
    stim_channel : str
        The stimulus channel that encodes HPI measurement intervals.
    %(verbose)s

    Returns
    -------
    %(chpi_locs)s

    Notes
    -----
    .. versionadded:: 0.23
    r^   c                    s   g | ]}ˆ j d  | ‘qS )rY   rZ   )Ú.0Úpickr]   r;   r<   Ú
<listcomp>ï   s    z)extract_chpi_locs_kit.<locals>.<listcomp>TF)ZstimÚmiscZref_megÚstim_channelZonset)rƒ   Úoutputr&   Nr   Úoffsetr2   z/Could not find appropriate cHPI intervals from r>   Úsfreqrd   Údirs)Úgoodz<u4)r:   z<f8)rQ   ÚsizeÚrÚcount)r‹   Ú z, truncating to z based on eventszFound z cHPI measurementr:   rc   rh   )#r+   Ú_RawKITr
   r[   Ústrr-   rY   Úindexr   rm   r‰   rr   r8   Z_raw_extrasr   ZDIR_INDEX_CHPI_DATAr4   ÚdtypeÚitemsizeÚAssertionErrorÚlistZ
_filenamesrH   ÚseekÚappendÚfromfileru   Úconcatenater'   r.   r   r%   rw   r5   rx   )r^   rƒ   r&   Zstim_chsÚidxZ	events_onZ
events_offÚbadrk   Ún_coilsÚheaderr�   Zall_datar9   rN   r:   Úextrari   rj   rl   r;   r]   r<   Úextract_chpi_locs_kitÚ   sŽ    
   ÿÿ  þ
þ  þ
þ
ÿÿ"
  ÿ ÿ
&2
r�   Úraisec                    sx  t | dtd� td|dddgd� t| d ƒd	ksJd
| d d	 d d	 krlt|dd� t d	¡dt d	¡fS t| d d d dd„ d�}t dd„ |D ƒ¡}t	 
dt|ƒd dd„ |D ƒ¡f ¡ | d }d}|dk	�r`d|k�rt| d |d gƒ}t|ƒd	k�r|d	 nd}dd„ |d D ƒ‰ t ‡ fdd„|D ƒ¡‰ t dd„ ˆ D ƒ¡}ˆ | ‰ || }nt t|ƒ¡‰ ||ˆ fS )a6  Retrieve cHPI information from the data.

    Parameters
    ----------
    %(info_not_none)s
    %(on_missing_chpi)s
    %(verbose)s

    Returns
    -------
    hpi_freqs : array, shape (n_coils,)
        The frequency used for each individual cHPI coil.
    hpi_pick : int | None
        The index of the ``STIM`` channel containing information about when
        which cHPI coils were switched on.
    hpi_on : array, shape (n_coils,)
        The values coding for the "on" state of each individual cHPI coil.

    Notes
    -----
    .. versionadded:: 0.24
    r[   )ÚitemZ	item_nameÚtypesÚ
on_missingÚignorerž   r*   )Z	parameterÚvalueZallowed_valuesZhpi_measr   Ú	coil_freqÚ	hpi_coilszSNo appropriate cHPI information found in info["hpi_meas"] and info["hpi_subsystem"])ÚmsgNr2   c                 S   s   | d S )NÚnumberr;   ©Úxr;   r;   r<   r_   P  r`   zget_chpi_info.<locals>.<lambda>ra   c                 S   s   g | ]}t |d  ƒ‘qS )r¤   )Úfloat)r   r©   r;   r;   r<   r�   S  s     z!get_chpi_info.<locals>.<listcomp>zUsing %s HPI coils: %s HzrB   c                 s   s   | ]}t t|ƒƒV  qd S ©N)rŽ   Úint)r   Úsr;   r;   r<   Ú	<genexpr>U  s     z get_chpi_info.<locals>.<genexpr>Zhpi_subsystemZevent_channelrY   c                 S   s   g | ]}|d  d ‘qS )Z
event_bitsr   r;   )r   Zcoilr;   r;   r<   r�   `  s     c                    s   g | ]}ˆ |d  d  ‘qS )r§   r   r;   )r   Zhc©Zhpi_onr;   r<   r�   b  s     c                 S   s   g | ]}|d k‘qS )r   r;   )r   Z	event_bitr;   r;   r<   r�   d  s     )r+   r	   r-   rm   r/   r4   Úemptyrn   rD   r'   r[   rK   r   rw   )r[   r¡   r&   r¥   Ú	hpi_freqsZhpi_subÚhpi_pickZhpi_maskr;   r¯   r<   Úget_chpi_info,  sB    ÿÿÿÿÿ

ÿ
r³   Fc                 C   s~  | d dkst | d ƒdkr$tdƒ‚| d d }tdd„ | d D ƒd	d
„ d�}t |ƒdkr¨t|d dd
„ d�}tdd„ |D ƒƒr¨|D ]"}|jt| d |d ƒtjd� q„| dt	 
dt |ƒd ¡¡d }| dt	 
t |ƒ¡¡}| dd¡}| dd¡}	| dt	 t |ƒ¡¡}
tdd„ |D ƒƒ�s(tdƒ‚t dt |ƒd dd„ |D ƒ¡f ¡ t d t |ƒd d!d„ |D ƒ¡f ¡ t	 d"d„ |D ƒ¡| }t |ƒd#k�sžt‚td$d„ | d d d D ƒd%d
„ d�}t	 d&d„ |D ƒ¡}|d' d( tjk�sðt‚t|d' d) |ƒ}d*|k�rLt d+|d* jddd…  ¡ |d* D ]}t d,t|ƒ ¡ �q2t	jj|| dd-�}t d.d/ d0d„ |D ƒ¡ ¡ | ¡ t |ƒ| k �r t d1¡ n¾|�sÂt |ƒt |ƒk�rÂtd2ƒ nœtt||ƒƒD ]Œ\}\}}d3| }|�s
||k�rZtd4|d |f ƒ nP|d |k�rÐ|
| |	k�rFt d5|d |f ¡ |||< ntd6|d |f ƒ �qÐt d7d3| |	f ¡ | t¡S )8zGet HPI fit locations from raw.Zhpi_resultsNr   z+no initial cHPI head localization performedr2   c                 S   s   g | ]}|d  t jkr|‘qS )Úkind)r   ÚFIFFV_POINT_HPI©r   Údr;   r;   r<   r�   t  s    ÿz(_get_hpi_initial_fit.<locals>.<listcomp>Údigc                 S   s   | d S ©NÚidentr;   r¨   r;   r;   r<   r_   v  r`   z&_get_hpi_initial_fit.<locals>.<lambda>ra   Z
dig_pointsc                 S   s   | d S r¹   r;   r¨   r;   r;   r<   r_   x  r`   c                 s   s"   | ]}|d  t jt jfkV  qdS ©Úcoord_frameN)r   ÚFIFFV_COORD_DEVICEZFIFFV_COORD_UNKNOWNr¶   r;   r;   r<   r®   y  s   þ
ÿz'_get_hpi_initial_fit.<locals>.<genexpr>Ú
dev_head_trŠ   )rŠ   r¼   Úorderr   ÚusedÚ
dist_limitç{®Gázt?Ú
good_limitç\�Âõ(\ï?Úgoodnessc                 s   s   | ]}|d  t jkV  qdS r»   )r   ÚFIFFV_COORD_HEADr¶   r;   r;   r<   r®   Š  s     zcHPI coordinate frame incorrectz&HPIFIT: %s coils digitized in order %srB   c                 s   s   | ]}t |d  ƒV  qdS )r   N©rŽ   )r   Úor;   r;   r<   r®   Ž  s     zHPIFIT: %s coils accepted: %sc                 s   s   | ]}t |ƒV  qd S r«   rÇ   )r   Úhr;   r;   r<   r®   �  s     c                 S   s   g | ]}|d  ‘qS ©rŠ   r;   r¶   r;   r;   r<   r�   ‘  s     rc   c                 S   s   g | ]}|‘qS r;   r;   r¶   r;   r;   r<   r�   •  s     c                 S   s   | d S r¹   r;   r¨   r;   r;   r<   r_   –  r`   c                 S   s   g | ]}|d  ‘qS rÊ   r;   r¶   r;   r;   r<   r�   —  s     Zcoord_transÚtoÚtransrl   zHpi coil moments (%d %d):z%g %g %grg   zHPIFIT errors:  %s mm.ú, c                 s   s   | ]}d d|  V  qdS )z%0.1fç     @�@Nr;   )r   Úer;   r;   r<   r®   ¤  s     z,HPI consistency of isotrak and hpifit is OK.z.HPI consistency of isotrak and hpifit is poor.éè  z9Discrepancy of HPI coil %d isotrak and hpifit is %.1f mm!z1Note: HPI coil %d isotrak is adjusted by %.1f mm!zJDiscrepancy of HPI coil %d isotrak and hpifit of %.1f mm was not adjusted!z.HP fitting limits: err = %.1f mm, gval = %.3f.)rm   r8   rn   rr   Úupdater   r   rÆ   Úgetr4   Úarangerv   r'   r[   rK   ÚdebugrD   r’   r5   rL   ÚlinalgÚnormÚsumr*   Ú	enumerateÚzipÚastyperª   )r[   Úadjustr&   Z
hpi_resultZhpi_digr¸   Z	pos_orderrÀ   rÁ   rÃ   rÅ   Zhpi_rrsZhpi_rrs_fitÚmomentÚerrorsÚhiÚerrZr_fitr·   r;   r;   r<   Ú_get_hpi_initial_fitm  sŒ    þþÿ ÿÿÿ
ÿÿ


ÿ
ÿ

ÿ

ÿrà   c                 C   sN   t | tj ||ƒ}t||||ƒ\}}	}
}|rJ||
 }t ||	j¡}||f}|S )z9Project data onto right eigenvectors of whitened forward.)r   r4   ÚnewaxisÚ_magnetic_dipole_deltaÚdotrt   )r©   ÚBÚB2ÚcoilsÚwhitenerÚ	too_closeÚreturn_momentÚfwdÚoutÚur­   ÚoneÚQr;   r;   r<   Ú_magnetic_dipole_objectiveÀ  s    rï   c           	      C   sL   t  | |j¡} t jj| dd�\}}}t  ||¡}t  ||¡}|| |||fS )NF©Zfull_matrices)r4   rã   rt   rÕ   Úsvd)	rê   rç   rä   rå   rì   r­   Úvrí   ÚBm2r;   r;   r<   râ   Ì  s
    râ   c                 C   s&   t  | |¡}t j|| dd�}|| S )Nr   rg   )r4   Úmatmulr×   )Úwhitened_fwd_svdrä   rå   rí   ró   r;   r;   r<   Ú_magnetic_dipole_delta_multiÖ  s    rö   c                 C   sÎ   ddl m} t || ¡}t ||¡}tt|||||d�}	|dk	r”|	|ƒ}
t|d ||ƒ}|j|d jd fksrt‚t 	|¡}|| |
k r”|d | }||	|ddd	d
d�}|	|dd�\}}d||  }|||fS )z$Fit a single bit of data (x0 = pos).r   )Úfmin_cobyla)rä   rå   ræ   rç   rè   Nrõ   Úrrr;   gü©ñÒMbP?gñhãˆµøä>F)ZrhobegZrhoendZdispT)ré   ç      ð?)
Zscipy.optimizer÷   r4   rã   r   rï   rö   r5   r’   Zargmin)ZB_origÚx0rè   rç   ræ   Úguessesr÷   rä   rå   Z	objectiveZres0Úresr˜   r©   ÚgofrÜ   r;   r;   r<   Ú_fit_magnetic_dipoleÝ  s.     þ  ÿ
rþ   c                 C   sB   t  |t| dd… ƒj¡}|| dd… 7 }||8 }||9 }| ¡ S )zCompute objective function.Nrc   )r4   rã   r!   rt   r×   )r©   Úcoil_dev_rrsÚcoil_head_rrsr·   r;   r;   r<   Ú_chpi_objectiveó  s
    r  c                 C   sL   t j |t j|dd� ¡}||9 }t| |ƒd }dt|| |ƒ|  }||fS )zDFit rotation and translation (quaternion) parameters for cHPI coils.r   rg   rù   )r4   rÕ   rÖ   Úmeanr#   r  )rÿ   r   ÚdenomÚquatrý   r;   r;   r<   Ú_fit_chpi_quatý  s
    r  Tc                 C   sÚ   t  d¡}d}d}|}t t  t|ƒ¡¡D ]œ}|t  |¡ }t| |ƒ\}	}
t j 	t
|	dd… ƒ¡dksjt‚|r¤t|	dd… t  d¡ƒ}|
td|t j  dƒd  }n|
}||kr*|
}|}t  |¡}|	}q*t|ƒ}|||fS )	zECompute Device to Head transform allowing for permutiatons of points.re   Niüÿÿrc   g§èH.ÿï?rù   r   g      Ð?)r4   rw   Ú	itertoolsÚpermutationsrÓ   rm   rD   r  rÕ   Zdetr!   r’   r    ÚmaxÚpir$   )Zdev_pntsZ	head_pntsZbiasZid_quatZ
best_orderZbest_gZ	best_quatZ
this_orderZhead_pnts_tmpÚ	this_quatÚgrV   Zcheck_gZout_gr¾   r;   r;   r<   Ú_fit_coil_order_dev_head_trans	  s&    
 
r  c                 C   s  |sdnd}t | |d�\}}}	t|tdfdƒ | d dk	rZt | d | d d	 | d ¡}
nt d
g¡}
t dd dd„ |
D ƒ¡ ¡ |  	d¡}|dkr¢| d d n|}||k}|rÄ|| }|	| }	n"| 
¡ sætd||   ¡ |f ƒ‚t|tƒ�rJtd|ddd� t|ƒ�rFt ||
f¡}t t |¡¡}td| ¡  d| ¡  ƒ}nd}t|ƒ}|d
k�rjtd|f ƒ‚t dd| f ¡ t || d  ¡ t¡}t||
| d |ƒ}tj |¡}t|t|ƒƒ}t| |ƒ\}}}t| ddd�}t| ddd�}t |||||||||	|||||
d�}|S ) z,Generate HPI structure for HPI localization.rž   r¢   )r¡   ÚnumericÚt_windowÚ	line_freqNr†   g      @r   z$Line interference frequencies: %s HzrB   c                 S   s   g | ]}d | ‘qS )z%dr;   )r   Úlfr;   r;   r<   r�   5  s     z0_setup_hpi_amplitude_fitting.<locals>.<listcomp>Zlowpassç       @zGFound HPI frequencies %s above the lowpass (or Nyquist) frequency %0.1f)Úautozif a string)rœ   g      @rù   gš™™™™™É?zt_window (%s) must be > 0zUsing time window: %0.1f msrÐ   ÚmagT)Úallow_emptyÚgrad)Ú	meg_picksÚ	mag_picksÚ
grad_picksr²   ÚmodelÚ	inv_modelr  Úinv_model_reordÚonÚn_windowÚprojÚproj_opÚfreqsÚ
line_freqs)!r³   r+   rŽ   r4   rÓ   rw   r'   r[   rK   rÒ   rr   r8   ÚtolistÚ
isinstancer-   rm   r—   rs   Úuniquer  Úminrª   rG   ZrintrÚ   r¬   Ú_setup_hpi_glmrÕ   ÚpinvÚ_reorder_inv_modelÚ_setup_ext_projr   rx   )r[   r  Úremove_aliasedÚ	ext_orderr  r&   r¡   r±   r²   Zhpi_onsr!  ZhighestZkeepersZ	all_freqsZdelta_freqsÚwindow_nsampr  r  r  r  r  r  r  r  Úhpir;   r;   r<   Ú_setup_hpi_amplitude_fitting&  sp    ÿÿ

þ

          ür.  c           	      C   s¸   t  dd|¡dd…t jf }dt j t j|td� | }| t jdd…f |dd…t jf  }|t jdd…f |dd…t jf  }t  |¡t  |¡t  |¡t  |¡|t  |¡g}t  	|¡S )z?Initialize a general linear model for HPI amplitude estimation.g      à¿g      à?Nr>   )r�   )
r4   Zlinspacerá   r	  rÓ   rª   ÚsinÚcosZ	ones_likeÚhstack)	r±   r!  r†   r,  ZslopeZradians_per_secZf_tZl_tr  r;   r;   r<   r&  a  s    $$  þr&  c                 C   s$   t  d| ¡ d|¡j ¡ }| | S )Nr>   )r4   rÓ   ru   rt   Zravel)r  Ún_freqsr˜   r;   r;   r<   r(  m  s    r(  c              
   C   s6  ddl m} t| dddd�}tt| ƒ|ƒ} t| d|ddd�\}}}}}t| dd	�}ttd
d|d�|ƒj	}t
dd|g ƒ}|t t t|ƒ¡|¡  }| |j	¡j	}|jd t|ƒks¶t‚ttjddt|| d | d t|ƒd�d�}	|  ¡ � |	g| d< W 5 Q R X t| dddd�\}
}|
jt|ƒfd k�s,t‚|	|
|fS )Nr   )rÕ   TFZbads)ÚmegZeegÚexcludeÚerror)Ú	int_orderr+  Z
ignore_refr&   r~   )ç        r7  r7  )Úoriginr6  r+  r   ZSSSZnchanrY   )r:   ZncolÚ	col_namesZnrow)r´   ÚdescZactiver:   Úprojs)Zadd_eeg_refÚactivater&   r>   )ZscipyrÕ   r
   r   r   r   r   r   rx   rt   r   r4   Zin1drÓ   rm   Zorthr5   r’   r   r   ZFIFFV_PROJ_ITEM_HOMOG_FIELDÚ_unlockr   )r[   r+  rÕ   r  Ú_Zmag_or_fineZmf_coilsÚextZout_removesr  r  r;   r;   r<   r)  t  sJ       þþ  ÿþ
   ÿ
r)  c                 C   s   d|    d¡S )zFormat log messages.z    t=%0.3f:é   )Úljust)Úfit_timer;   r;   r<   Ú_time_prefix�  s    rC  c              	   C   s   t dƒ� | |d |f d }W 5 Q R X |d dk	r®t dƒ� | |d |f d }W 5 Q R X t |¡ tj¡|d dd…tjf @  t¡}|jdd�jdd�}|d	k ¡ s®dS |rÜt	|t
|d
 ƒ|d |d |d |d ƒS t||d t
|d
 ƒ|d |d ƒS )a~  Fit amplitudes for each channel from each of the N cHPI sinusoids.

    Returns
    -------
    sin_fit : ndarray, shape (n_freqs, n_channels)
        The sin amplitudes matching each cHPI frequency.
        Will be all nan if this time window should be skipped.
    snr : ndarray, shape (n_freqs, 2)
        Estimated SNR for this window, separately for mag and grad channels.
    Fr  r   r²   Nr  r2   rg   rc   r   r  r  r  r  r  r  )r(   r4   ÚroundrÚ   Zint64rá   Úboolrr   r×   Ú_fast_fit_snrrm   Ú	_fast_fit)r^   ry   r-  ÚsnrÚ	this_datarz   ZonsZn_onr;   r;   r<   Ú_fit_chpi_amplitudes”  s4    

ÿÿ 
   þ ÿrJ  c                 C   s®   | j d |j d kr8|d | j d … }ttj |¡|ƒ}||  }||j }t ||j d f¡}t|ƒD ]B}tjj|d| d| d … dd�\}	}
}|d |
d  ||< qf|S )Nr   r   r>   Frð   )	r5   r(  r4   rÕ   r'  rt   rw   Úrangerñ   )rI  r  r2  r  r  Z	proj_dataÚXÚsin_fitÚfirì   r­   Úvtr;   r;   r<   rG  ·  s    
*rG  c              	   C   sV  | j d |j d kr2|d | j d … }tj |¡}t |¡t | j¡ }|d |… d ||d| … d  d }| t || j¡ }t |jdd�|j d  ¡j}	t ||	 ¡d }
|
jdd�|
j d  }t 	|df¡}||fD ]p}t
|ƒrà|d d …|f jdd�t
|ƒ }||  ¡ t |¡ }dt || ¡ }t |t |||fd¡f¡}qà|S )Nr   r   r>   rg   r3   )r5   r4   rÕ   r'  Zascontiguousarrayrt   Z
atleast_2dr×   Úabsr°   rm   r  rv   Úlog10r1  Ústack)rI  r2  r  r  r  r  ZcoefsZ	hpi_powerÚresidZ
resid_meanZsquared_devsZ	resid_varZsnrsZ_picksZ	avg_powerZ	avg_residrH  r;   r;   r<   rF  Ê  s"    ( rF  c                 C   s  |dkr t ddddd�}tƒ }n|dks,t‚t ddd�}dg}t| t |ƒ t| ¡ ƒ| }t|ƒ | ¡r‚td	||t|  ¡ ƒf ƒ‚d }| 	¡ D ]Š\}}d
||f }| | }	t|	t
j|ƒ |	j}
|	j|krÜtd|||	jf ƒ‚|d krô|dkrô|
d }||
d krŽ|dkrŽtd||f ƒ‚qŽ|dk�r¤| d jd }dD ]6}| | }	|	jd |k�r6td|||	jd f ƒ‚�q6dD ].}| | }	|	jd dk�rrtd||
f ƒ‚�qrnp|dk�s²t‚| d | d  }}t|tdƒ t|d d ƒ}|jd |k�s |jd |k�rtd|||jf ƒ‚d S )NÚ	chpi_locsr   rc   r>   )rk   ri   rl   rj   Úchpi_amplitudes)rk   Úslopesr  z)%s must be a dict with entries %s, got %sz%s[%s]z%s must have ndim=%d, got %dr   z0%s have inconsistent number of time points in %sri   )rj   rl   zTchpi_locs["rrs"] had values for %d coils but chpi_locs["%s"] had values for %d coils)ri   rl   z0chpi_locs["%s"].shape[2] must be 3, got shape %srV  zchpi_amplitudes["proj"]r:   r9  z<slopes must have shape[0]==%d and shape[2]==%d, got shape %s)rx   r“   r’   r+   ÚkeysÚsetÚsymmetric_differencerG   rn   Úitemsr4   Zndarrayr5   rF   r   rm   )Zchpi_ÚnameZ
want_ndimsZ
extra_keysZ	want_keysÚn_timesrb   Z	want_ndimZkey_strÚvalr5   rš   rV  r  Zn_chr;   r;   r<   Ú_check_chpi_paramæ  sd    ÿ

ÿÿ
þ
ÿ 
ÿr^  rÂ   rÄ   c           "         s  t ˆ dƒ t| tdƒ t| |dd�}t|ƒ}tt| d ƒ|ƒ}| d d }	|	dd…df }
td	|t 	t
|	dd…dd…f ƒ|	dd…df g¡d
�}~g }t‡ fdd„dD ƒŽ D �]&\}}}t ||k¡d }t|ƒdk �rt|ƒdt|ƒ|d dd„ |D ƒ¡f  }t|ƒ q®t|||ƒ\}}}t|ƒ}t||ƒ}tjj|| dd�}||k||k @  ¡ }|dk �r–tt|ƒd||d dd„ t||ƒD ƒ¡f  ƒ q®||d  }tdtjj|d | dd� | ƒ}t t|ƒdd dg| ¡ ||f|   ¡ t|ƒD ]æ}||k�rd\}}nd\}}d| d d  d! | }t 	d"||  d"||  || d"||  gf¡}t|ƒdk�rÐ|d#k�r |d$7 }t 	|||dd…f f¡}n0|dk�rÐ|d%7 }t 	||dd…df d f¡}t |j|Ž ¡ �qütj |d& dd… |dd…  ¡}t|d& dd… |dd… ƒ| } || }!d'tj |dd… |
 ¡ }t d(|d'| ¡  |d'|! | |f ¡ t d)|d* td+j|ƒ¡f ¡ | t 	|g||g||  ¡ g|!gf¡¡ ||d< ||d&< ||d< q®t |tj ¡}|j!dk�rüt "d,¡n|}|S )-aÚ  Compute time-varying head positions.

    Parameters
    ----------
    %(info_not_none)s
    %(chpi_locs)s
        Typically obtained by :func:`~mne.chpi.compute_chpi_locs` or
        :func:`~mne.chpi.extract_chpi_locs_ctf`.
    dist_limit : float
        Minimum distance (m) to accept for coil position fitting.
    gof_limit : float
        Minimum goodness of fit to accept for each coil.
    %(adjust_dig_chpi)s
    %(verbose)s

    Returns
    -------
    quats : ndarray, shape (n_pos, 10)
        The ``[t, q1, q2, q3, x, y, z, gof, err, v]`` for each fit.

    See Also
    --------
    compute_chpi_locs
    extract_chpi_locs_ctf
    read_head_pos
    write_head_pos

    Notes
    -----
    .. versionadded:: 0.20
    rT  r[   r5  )rÛ   r&   r¾   rÌ   Nrc   gš™™™™™¹¿)Úquat_fit_timerÿ   r  c                 3   s   | ]}ˆ | V  qd S r«   r;   ©r   rb   ©rT  r;   r<   r®   J  s     z#compute_head_pos.<locals>.<genexpr>)rk   ri   rj   r   zB%s/%s good HPI fits, cannot determine the transformation (%s GOF)!rÍ   c                 s   s   | ]}d | V  qdS )z%0.2fNr;   )r   r  r;   r;   r<   r®   T  s     r   rg   zE%s/%s good HPI fits, cannot determine the transformation (%s mm/GOF)!c                 s   s(   | ] \}}d | d›d|d›�V  qdS )rÐ   z0.1fz::z0.2fNr;   )r   rÏ   r  r;   r;   r<   r®   m  s   ÿr_  rÎ   rÿ   z(%s/%s good HPI fits, movements [mm/s] = z / z% 8.1f)rB   ú/)ú(ú)z    z{0:6.1f} {1:6.1f} {2:6.1f} / z{3:6.1f} {4:6.1f} {5:6.1f} / zg = {6:0.3f} err = {7:4.1f} rÐ   r>   z{8:6.3f} {9:6.3f} {10:6.3f}z{8:6.1f} {9:6.1f} {10:6.1f}r  éd   z[    #t = %0.3f, #e = %0.2f cm, #g = %0.3f, #v = %0.2f cm/s, #r = %0.2f rad/s, #d = %0.2f cmz    #t = %0.3f, #q = %s rB   z{:8.5f})r   r3   )#r^  r+   r	   rà   rm   r   r   rx   r4   r—   r"   rÙ   rq   rC  rK   r*   Ú_fit_chpi_quat_subsetr$   rÕ   rÖ   r×   rL   r'   r[   rK  rÔ   Úformatr    r  Úmapr•   rD   rE   r‰   rw   )"r[   rT  rÁ   Z	gof_limitÚ
adjust_digr&   Zhpi_dig_head_rrsrš   rÿ   r¾   Zpos_0ÚlastrT   rB  Zthis_coil_dev_rrsZg_coilsÚuse_idxr¦   r
  r  Zthis_dev_head_tZest_coil_head_rrsZerrsZn_goodÚdtÚvsÚiiÚstartÚendZlog_strÚvalsr·   rŠ   rò   r;   ra  r<   Úcompute_head_pos  sà    "
ÿÿÿÿÿÿþ  ÿ




ÿÿþÿÿÿ
ÿ
þÿ

ÿþýü
þ
ÿ
ÿ$"þÿ ÿ
rr  c           
      C   sœ   t | | || ƒ\}}| ¡ }t|ƒdkrŠtt|ƒƒD ]R}t|d |… ƒt||d d … ƒ }t| ||ƒ\}}	}|	|kr6||	|  }}}q6||t |t¡fS )Nrc   r   )	r  rS   rm   rK  r“   rf  r4   rD   r¬   )
rÿ   r   rk  r  r  Zout_idxZdiZthis_use_idxr
  Zthis_gr;   r;   r<   rf  ©  s    $  ÿ
rf  c                 C   s   d| |    ¡  S )zEConstrain our 3 quaternion rot params (ignoring w) to have norm <= 1.r   )r×   r¨   r;   r;   r<   Ú_unit_quat_constraint¶  s    rs  ç{®Gáz„?r  c              
   C   s   t | ||||||dd�S )aß  Compute time-varying estimates of cHPI SNR.

    Parameters
    ----------
    raw : instance of Raw
        Raw data with cHPI information.
    t_step_min : float
        Minimum time step to use.
    %(t_window_chpi_t)s
    %(ext_order_chpi)s
    %(tmin_raw)s
    %(tmax_raw)s
    %(verbose)s

    Returns
    -------
    chpi_snrs : dict
        The time-varying cHPI SNR estimates, with entries "times", "freqs",
        "snr_mag", "power_mag", and "resid_mag" (and/or "snr_grad",
        "power_grad", and "resid_grad", depending on which channel types are
        present in ``raw``).

    See Also
    --------
    mne.chpi.compute_chpi_locs, mne.chpi.compute_chpi_amplitudes

    Notes
    -----
    .. versionadded:: 0.24
    T)rH  ©Ú_compute_chpi_amp_or_snr©r^   Ú
t_step_minr  r+  ÚtminÚtmaxr&   r;   r;   r<   Úcompute_chpi_snr¼  s    !
   ÿr{  c                 C   s   t | ||||||ƒS )a.  Compute time-varying cHPI amplitudes.

    Parameters
    ----------
    raw : instance of Raw
        Raw data with cHPI information.
    t_step_min : float
        Minimum time step to use.
    %(t_window_chpi_t)s
    %(ext_order_chpi)s
    %(tmin_raw)s
    %(tmax_raw)s
    %(verbose)s

    Returns
    -------
    %(chpi_amplitudes)s

    See Also
    --------
    mne.chpi.compute_chpi_locs, mne.chpi.compute_chpi_snr

    Notes
    -----
    This function will:

    1. Get HPI frequencies,  HPI status channel, HPI status bits,
       and digitization order using ``_setup_hpi_amplitude_fitting``.
    2. Window data using ``t_window`` (half before and half after ``t``) and
       ``t_step_min``.
    3. Use a linear model (DC + linear slope + sin + cos terms) to fit
       sinusoidal amplitudes to MEG channels.
       It uses SVD to determine the phase/amplitude of the sinusoids.

    In "auto" mode, ``t_window`` will be set to the longer of:

    1. Five cycles of the lowest HPI or line frequency.
          Ensures that the frequency estimate is stable.
    2. The reciprocal of the smallest difference between HPI and line freqs.
          Ensures that neighboring frequencies can be disambiguated.

    The output is meant to be used with :func:`~mne.chpi.compute_chpi_locs`.

    .. versionadded:: 0.20
    ru  rw  r;   r;   r<   Úcompute_chpi_amplitudesá  s
    0
  ÿr|  c                 C   sÀ  t | j||d�}|  ||¡\}}|| jd  }|| jd  }|d d }	| jt ||	 ||¡dd�}
t dt|d ƒt|
ƒ|| f ¡ ~~tƒ }|d	 |d	< t 	|
| j
 |d
 d  ¡| jd  |d< t|d ƒ}t|d ƒ}t|d	 d d ƒ}|�rx|d	= |d |d< |  ¡ }d|k�r"dnd}dD ]J}||k�r*dD ]4}|dk�rNdn|}t ||f¡||› d|› �< �q<�q*nt |||f¡|d< d|�r˜dnd› �}tt|
|d�ƒD �]\}}||d
 d  }tt|dƒt||d
  t| jƒƒƒ}t| |||ƒ}|�r¬|  ¡ }d|k�rT|dd…df |d |< |dd…df |d |< |d |d  |< d!|k�r¸|dd…|f |d" |< |dd…|d f |d# |< |d|d f |d$ |< n||d |< �q°|S )%zÅCompute cHPI amplitude or SNR.

    See compute_chpi_amplitudes for parameter descriptions. One additional
    boolean parameter ``snr`` signals whether to return SNR instead of
    amplitude.
    )r+  r†   r  r  T)Zuse_roundingzJFitting %d HPI coil locations at up to %s time points (%0.1f sec duration)r   r  r  rk   r:   r9  r  rc   r   )r  r  )rH  ÚpowerrS  rS  r   r>  rV  zcHPI ZSNRsZ
amplitudes©Zmesgr>   NZmag_snrZ	mag_power)r   r>   Z	mag_residr  Zgrad_snrZ
grad_powerZ
grad_resid)r.  r[   Z_tmin_tmax_to_start_stopZtime_as_indexr4   rÓ   r'   rm   rx   rD  Z
first_sampZget_channel_typesr°   rØ   r,   ro   r  r%  rk   rJ  )r^   rx  r  r+  ry  rz  r&   rH  r-  Zneed_winÚfit_idxsÚsin_fitsr\  r2  Zn_chansZch_typesZgrad_offsetZch_typerb   ÚcolsÚmessageÚmiÚmidptry   Zamps_or_snrsr;   r;   r<   rv    sp    	  ÿÿþ
ÿÿ
&
ÿ

rv  rù   c              	      s0  t dˆdddgƒ t|dƒ t| tdƒ |}~|d }t| d |d d	 d
d�}t| |ƒ} |  ¡ � |g| d< W 5 Q R X ~~tt| d dƒƒ‰t	| dd�}	t
|	| dd�\‰}
tjjˆd dd� ¡ }tt|t d¡d�ddddd�d d ‰ t dtˆ ƒ|d f ¡ tˆ ˆˆƒ}t |ˆj¡}ˆ jd ddf|_tjj|dd�d }tˆ |d�‰ ~~tt|d  |d! ƒƒ}tg g g g d"�}tt| d# ƒd$ t| |d%�ƒ}td&|d  d d |d'�}t|ƒ}~t|d(d)�D �]\}}t  |¡ !¡ �sè�qÊ|d* d&k	�rHt "d+d,„ t||d* ƒD ƒ¡}||9 }||d-  |d. k�rH|d/k #¡ dk�rH�qÊ| $¡ |d*< ‡ ‡‡‡fd0d,„t||d1 ƒD ƒ}t|Ž \}}}|d   %|¡ |d2  %|¡ |d3  %|¡ |d4  %|¡ ||d-< ||d1< �qÊt|d  ƒ}t|f||df||f||dfd"�}| &¡ D ]$\}}t "|t'¡ (|| ¡||< �q|S )5uÈ  Compute locations of each cHPI coils over time.

    Parameters
    ----------
    %(info_not_none)s
    %(chpi_amplitudes)s
        Typically obtained by :func:`mne.chpi.compute_chpi_amplitudes`.
    t_step_max : float
        Maximum time step to use.
    too_close : str
        How to handle HPI positions too close to the sensors,
        can be 'raise' (default), 'warning', or 'info'.
    %(adjust_dig_chpi)s
    %(verbose)s

    Returns
    -------
    %(chpi_locs)s

    See Also
    --------
    compute_chpi_amplitudes
    compute_head_pos
    read_head_pos
    write_head_pos
    extract_chpi_locs_ctf

    Notes
    -----
    This function is designed to take the output of
    :func:`mne.chpi.compute_chpi_amplitudes` and:

    1. Get HPI coil locations (as digitized in ``info['dig']``) in head coords.
    2. If the amplitudes are 98%% correlated with last position
       (and Î”t < t_step_max), skip fitting.
    3. Fit magnetic dipoles using the amplitudes for each coil frequency.

    The number of fitted points ``n_pos`` will depend on the velocity of head
    movements as well as ``t_step_max`` (and ``t_step_min`` from
    :func:`mne.chpi.compute_chpi_amplitudes`).

    .. versionadded:: 0.20
    rè   rž   Úwarningr[   rU  r  rY   r:   r9  T)Úorderedr;  ZchsZaccurateFr~   r   r   rg   rc   )ÚRZr0rt  r7  rÂ   rø   zBComputing %d HPI location guesses (1 cm grid in a %0.1f cm sphere)re  r2   rð   r>   )rø   rõ   rk   rV  )rk   ri   rj   rl   r¾   rÌ   )rÛ   N)rM  Úcoil_fit_timerÿ   zcHPI locations r~  rM  c                 S   s    g | ]\}}t  ||¡d  ‘qS ))r   r   )r4   Zcorrcoef)r   r­   Úlr;   r;   r<   r�   ½  s   ÿz%compute_chpi_locs.<locals>.<listcomp>rˆ  gH¯¼šò×z>rÄ   c              	      s"   g | ]\}}t ||ˆˆˆˆ ƒ‘qS r;   )rþ   )r   Úfrú   ©rû   Z	meg_coilsrè   rç   r;   r<   r�   Í  s
   þ
 ÿrÿ   ri   rj   rl   ))r-   r^  r+   r	   r   r   r=  r   r   r   r   r4   rÕ   rÖ   r%  r   rx   rw   r'   r[   rm   r   rã   rt   r5   rñ   r“   rÙ   r   r   rà   r,   Úisfiniterr   rD   r×   rS   r•   rZ  rª   ru   )r[   rU  Z
t_step_maxrè   ri  r&   r€  r  r  Úcovr>  r‡  rê   Ziter_rT  Zhpi_dig_dev_rrsrj  Zn_hpirB  rM  ZcorrsZ	coil_fitsri   rj   rl   r\  Zshapesrb   r]  r;   r‹  r<   Úcompute_chpi_locs]  s¢    /
 
 ÿ

ÿÿÿÿ
þÿÿÿÿþürŽ  c                    sR   t ƒ }t‡ fdd„dD ƒŽ D ](\}}| dd„ tt||ƒdƒD ƒ¡ qˆ d |fS )z)Reformat chpi_locs as list of dig (dict).c                 3   s   | ]}ˆ | V  qd S r«   r;   r`  ra  r;   r<   r®   æ  s     z*_chpi_locs_to_times_dig.<locals>.<genexpr>)ri   rj   c                 S   s(   g | ] \}\}}|||t jt jd œ‘qS ))rŠ   rº   rý   r´   r¼   )r   rµ   r½   )r   r˜   rø   rý   r;   r;   r<   r�   ç  s
   
ýþz+_chpi_locs_to_times_dig.<locals>.<listcomp>r   rk   )r“   rÙ   r•   rØ   )rT  r¸   ri   rj   r;   ra  r<   Ú_chpi_locs_to_times_digã  s    
ý
r�  c           "   
   C   s  t | tdƒ | jstdƒ‚t|ƒ}|dkr8td|f ƒ‚tt || j	d  ¡ƒ}|rj| j	d dkrjtdƒ‚t
| j	|d	||d
d�}t dt| jƒ|d d  |¡}	t|d ƒ}
d|
 }t| j	d	dd�}t| jƒ}d|
 }|�r|dt|d ƒ 7 }|dt|d ƒ 7 }|dt|ƒ 7 }t |d dd…d|…f |d d|… ¡j}t 	|¡ tƒ }d}t|	dd�}t|ƒD �]œ\}}||d d  }tt|dƒt||d  t| jƒƒƒ}|j|j }||d k�rÈ|}nB|d d|… }tj |¡}t |dd…d|…f |d|… ¡j}| j||f }t|| |ƒ}||k�rz||j |d d  }t||||  ƒ}| |t ||dd…|f ¡f¡ |}|t|	ƒd k �rª|	|d  |d d  }ntj}t|ƒdk�rj|d d |k�rj| d¡\} }!| j|| |!j d  | …f  |!8  < �q°�qj| S )aM  Remove cHPI and line noise from data.

    .. note:: This function will only work properly if cHPI was on
              during the recording.

    Parameters
    ----------
    raw : instance of Raw
        Raw data with cHPI information. Must be preloaded. Operates in-place.
    include_line : bool
        If True, also filter line noise.
    t_step : float
        Time step to use for estimation, default is 0.01 (10 ms).
    %(t_window_chpi_t)s
    %(ext_order_chpi)s
    allow_line_only : bool
        If True, allow filtering line noise only. The default is False,
        which only allows the function to run when cHPI information is present.

        .. versionadded:: 0.20
    %(verbose)s

    Returns
    -------
    raw : instance of Raw
        The raw data.

    Notes
    -----
    cHPI signals are in general not stationary, because head movements act
    like amplitude modulators on cHPI signals. Thus it is recommended to
    to use this procedure, which uses an iterative fitting method, to
    remove cHPI signals, as opposed to notch filtering.

    .. versionadded:: 0.12
    r^   zraw data must be preloadedr   zt_step (%s) must be > 0r†   r  Nz^include_line=True but raw.info["line_freq"] is None, consider setting it to the line frequencyTF)r*  r+  r  r&   r  r>   r   r;   )r3  r4  zRemoving %s cHPIr!  z and %s line harmonicz! frequencies from %s MEG channelsr  r  Z	Filteringr~  r   )!r+   r   Zpreloadr8   rª   rG   r¬   r4   Úceilr[   r.  rÓ   rm   rk   r
   rã   rt   r'   r“   r,   rØ   ro   r  r%  Ústopro  rÕ   r'  Ú_datar•   ÚinfÚpopr5   )"r^   Zinclude_lineZt_stepr  r+  Zallow_line_onlyr&   Zn_stepr-  r  r2  Zn_remover  r\  r¦   ZreconÚchunksZ
last_endptZpbrn  r„  Z	left_edgery   Zthis_lenZ
this_reconr  r  rI  Zsubt_ptZfit_left_edgeZfit_slZnext_left_edgeZ
right_edgeÚchunkr;   r;   r<   Úfilter_chpiî  s€    '    þ 
.

ÿ&

ÿ"  ÿÿr—  c                 C   sº   ddl m} |||ƒ}t | | ¡}d}t t| ƒt¡}|s²|| dd…|f }||k}| ¡  }|s8| ¡ dkr€d|dd…< q²|| jdd�}	t 	|¡d t 
|	¡ }
d||
< q8||fS )z&Compute good coils based on distances.r   )ÚcdistFNr>   rg   )Zscipy.spatial.distancer˜  r4   rP  rv   rm   rE  r7   r×   rq   Zargmax)Zhpi_coil_distsÚnew_posrÁ   r˜  Zthese_distsrˆ   Zuse_maskr·   Zd_badZbadnessZexclude_coilsr;   r;   r<   Ú_compute_good_distancesU  s"    


rš  )N)r}   )rž   N)FN)F)T)Fr   FN)F)rÂ   rÄ   FN)rt  r  r   r   NN)rt  r  r   r   NN)rt  r  r   r   NNF)rù   rž   FN)Trt  r  r   FN)rÂ   )dÚ__doc__Ú	functoolsr   Únumpyr4   r  Úeventr   Zio.baser   Zio.kit.constantsr   Z
io.kit.kitr   r�   Zio.meas_infor   r	   Zio.pickr
   r   r   r   r   Zio.projr   r   Zio.constantsr   Zio.ctf.transr   Úforwardr   r   r   r�  r   r   Zdipoler   Úfixesr   Zpreprocessing.maxwellr   r   r   r   Z
transformsr   r   r    r!   r"   r#   r$   r%   Úutilsr&   r'   r(   r)   r*   r+   r,   r-   r.   r/   r=   rP   rX   r|   r�   r³   rà   rï   râ   rö   rþ   r  r  r  r.  r&  r(  r)  rC  rJ  rG  rF  r^  rr  rf  rs  r{  r|  rv  rŽ  r�  r—  rš  r;   r;   r;   r<   Ú<module>   sÈ   (0  AQ@S ÿ

	
	
      ÿ:

#

3    ÿ 
      ÿ$        ÿ3         þ
H    ÿ       ÿf