U
    f»{j~P  ã                   @   s  d dl mZ d dlmZ d dlZddlmZ ddlm	Z	m
Z
 ddlmZmZmZ ddlmZ dd	lmZmZmZmZmZmZmZmZmZ d
dlmZmZmZmZm Z m!Z! ed&dd„ƒZ"dd„ Z#dd„ Z$dd„ Z%dd„ Z&d'dd„Z'd(dd„Z(dd„ Z)d d!„ Z*d"d#„ Z+d$d%„ Z,dS ))é    )Údefaultdict)ÚpartialNé   )Ú_check_origin)Ú	pick_infoÚ
pick_types)Ú_loc_to_coil_transÚ_coil_trans_to_locÚBaseRaw)Ú_find_vector_rotation)	ÚloggerÚverboseÚcheck_fnameÚ_check_fnameÚ_plÚ_ensure_intÚ_check_optionÚ_validate_typeÚ	_reg_pinvé   )Ú_col_norm_pinvÚ_trans_sss_basisÚ_prep_mf_coilsÚ_get_grad_point_coilsetsÚ_read_cross_talkÚ_prep_fine_calé   ç      $@©ç        r   r   c           &         s–  t |dƒ}td|dƒ t| tdƒ t |dƒ}t|| jddd�}td| jd	 g fƒ t| jdd
d�}| jd dk	rxtdƒ‚t| j|ƒ}	t|	ddd�‰t|	ddd�}
t	||	d ƒ\}}t|t
dfdƒ t t|	d ƒ¡‰ |  t d| jd |¡¡}t|ƒdk�rt dt| jƒgt¡}nt| jƒ|d< d}t dd„ |	d D ƒ¡}|ˆdd…f  ¡ }|dk	�rÂt|	|ƒ\}}}tˆƒD ]H\}}|d  |	d | ¡}|d | ˆ |< |d | dd… ||< �qt�ntˆƒdk�rÞtƒ }tƒ }t dtˆƒt|ƒd f ¡ t|dd… |dd… ƒD ]‚\}}t d ||	d!  ||	d!  f ¡ | |||…f d }|dk	�rf| |¡}t|	|||ƒ\}}}|�r| |¡ | |¡ �qt|ƒ}|dk�r²td"ƒ‚tj|dd#�ˆ dd…< tj|dd#�|dd…< tˆƒdk�r"t|ƒD ],\}}|ˆ|  }t||ƒ}t |||ƒ �qôt|ƒD ]"\} }!|!|	d |  d$ dd…< �q*~~t|
ƒdk�r*|dk�rnd%nd&}"t d't|
ƒ|"f ¡ tƒ }#t|dd… |dd… ƒD ]p\}}t d ||	d!  ||	d!  f ¡ | |||…f d }|dk	�rú| |¡}t!|	|ˆ |||ƒ}$|# |$¡ �q¨tj|#dd#�‰nt "t|	d ƒ|f¡‰tt #ˆ|
¡ƒdk�sZt$‚‡ ‡‡fd(d„t%t|	d ƒƒD ƒ}%t
|	d ||%d)�}||fS )*a�	  Compute fine calibration from empty-room data.

    Parameters
    ----------
    raw : instance of Raw
        The raw data to use. Should be from an empty-room recording,
        and all channels should be good.
    n_imbalance : int
        Can be 1 or 3 (default), indicating the number of gradiometer
        imbalance components. Only used if gradiometers are present.
    t_window : float
        Time window to use for surface normal rotation in seconds.
        Default is 10.
    %(ext_order_maxwell)s
        Default is 2, which is lower than the default (3) for
        :func:`mne.preprocessing.maxwell_filter` because it tends to yield
        more stable parameter estimates.
    %(origin_maxwell)s
    %(cross_talk_maxwell)s
    calibration : dict | None
        Dictionary with existing calibration. If provided, the magnetometer
        imbalances and adjusted normals will be used and only the gradiometer
        imbalances will be estimated (see step 2 in Notes below).
    %(verbose)s

    Returns
    -------
    calibration : dict
        Fine calibration data.
    count : int
        The number of good segments used to compute the magnetometer
        parameters.

    See Also
    --------
    mne.preprocessing.maxwell_filter

    Notes
    -----
    This algorithm proceeds in two steps, both optimizing the fit between the
    data and a reconstruction of the data based only on an external multipole
    expansion:

    1. Estimate magnetometer normal directions and scale factors. All
       coils (mag and matching grad) are rotated by the adjusted normal
       direction.
    2. Estimate gradiometer imbalance factors. These add point magnetometers
       in just the gradiometer difference direction or in all three directions
       (depending on ``n_imbalance``).

    Magnetometer normal and coefficient estimation (1) is typically the most
    time consuming step. Gradiometer imbalance parameters (2) can be
    iteratively reestimated (for example, first using ``n_imbalance=1`` then
    subsequently ``n_imbalance=3``) by passing the previous ``calibration``
    output to the ``calibration`` input in the second call.

    MaxFilter processes at most 120 seconds of data, so consider cropping
    your raw instance prior to processing. It also checks to make sure that
    there were some minimal usable ``count`` number of segments (default 5)
    that were included in the estimate.

    .. versionadded:: 0.21
    Ún_imbalance)r   r   ÚrawÚ	ext_orderÚmegT)Údispzraw.info['bads']ÚbadsF)r#   Zref_megZ
dev_head_tNz]info["dev_head_t"] is not None, suggesting that the data are not from an empty-room recordingÚmag© ©r#   ÚexcludeÚgradÚch_namesÚcalibrationr   éÿÿÿÿr   r   c                 S   s   g | ]}|d  ‘qS ©Úlocr'   ©Ú.0Úchr'   r'   úZ/var/www/html/bwave/python/venv/lib/python3.8/site-packages/mne/preprocessing/_fine_cal.pyÚ
<listcomp>z   s     z,compute_fine_calibration.<locals>.<listcomp>ÚchséýÿÿÿÚimb_calsÚlocszIAdjusting normals for %s magnetometers (averaging over %s time intervals)z)    Processing interval %0.3f - %0.3f secZsfreqzNo usable segments found©Úaxisr/   zX directionzXYZ directionsz+Computing imbalance for %s gradimeters (%s)c                    s,   g | ]$}|ˆkr ˆ ||d  … nˆ| ‘qS )r   r'   )r1   Úii©ÚcalsÚimbÚ	mag_picksr'   r3   r4   »   s   ÿ©r+   r8   r7   )&r   r   r   r
   r   Úinfor   Ú
ValueErrorr   r   ÚdictÚnpÚonesÚlenZtime_as_indexZarangeÚtimesÚarrayÚintÚcopyr   Ú	enumerateÚindexÚlistr   ÚzipÚdotÚ_adjust_mag_normalsÚappendÚRuntimeErrorZmeanÚ_matched_loc_idxÚ_rotate_locsÚ_estimate_imbalanceÚzerosZintersect1dÚAssertionErrorÚrange)&r!   r    Zt_windowr"   ÚoriginZ
cross_talkr,   r   ZpicksrA   Ú
grad_picksZctcÚ_Z	time_idxsÚcountr8   ÚzsÚpiÚpickÚidxZcal_listZz_listÚstartÚstopÚdataÚzÚcalÚgoodr;   Únew_zZz_locÚidxsÚcir/   ÚextraZimb_listÚoutr7   r'   r<   r3   Úcompute_fine_calibration   s®    C

ÿ
 þ"ÿ





ÿ"ÿ

  ÿÿrl   c                    s   t  ‡ fdd„|D ƒ¡d S )Nc                    sD   g | ]<}t  ˆ d d… |d d… ¡o>t  ˆ dd… |dd… ¡‘qS )r6   Nr   )rD   Úallclose)r1   r/   ©Úmag_locr'   r3   r4   Â   s   ÿz$_matched_loc_idx.<locals>.<listcomp>r   )rD   Úwhere)ro   Zall_locr'   rn   r3   rS   Á   s
    ÿÿrS   c                 C   s¶   |t j |¡ }| |d  dd … }|t j |¡ }t||ƒ}|D ]n}t| | ƒ}t  ||d d…d d…f ¡|d d…d d…f< t|ƒ| | d d …< t jj| | dd … |dd� qBd S )Nr   r6   r   ç-Cëâ6?)Zatol)	rD   ÚlinalgÚnormr   r   rO   r	   ZtestingZassert_allclose)r8   rh   rg   Úold_zÚrotri   Z
this_transr'   r'   r3   rT   Æ   s    
,rT   c                 C   sF   t  t  t  | | jdd�t jj| dd�t jj|dd�  dd¡¡¡S )z-Get the angle between two vectors in degrees.r-   r9   ç      ð?)rD   ÚabsZarccosZclipÚsumrr   rs   )ÚxÚyr'   r'   r3   Ú_vector_angleÒ   s    ÿÿ þÿr{   c           )      C   s¬  ddl m} d}t| ddd�}t| ddd�}t| ddd�}t| |ƒ} || }t t|ƒd	f¡}	t t|	ƒ¡}
t| dd
�}||  |9  < td||d�}t	| dd�}t
|||d�}t|||	ƒ}d}t dd„ | d D ƒ¡}|tjj|ddd� }| ¡ }tƒ }t dd„ | d D ƒ¡}|D ]}t|| |ƒ||< �qtdd„ ƒ}tdƒD �]\}}t d|› d�¡ t
|||d�}|D �]Ú}t|||	d	d�}|dk�ròtt || ¡ƒ}d}t|ƒdk�rÜ|| d¡ }|| dk �rª�qÜ�qª|| dk �rö �qFn|}||  d	7  < ||k�st‚|d	7 }||  ¡ }tt|||||	||||d�
}|||ddddd �||< d!tj || ¡ |	|< ||  |	| 9  < || D ]}t|| |||dd"� �q�t
||ƒ}|| }t|||	|d#�} t t t|| || ƒ¡¡|
|< | d$ | }!t d%|d&›d'|!› d(|| › d)|d*›d+| d*›d,|	|df d-›d'|
| d.›d/�¡ �qt�qFt|||	ƒ}"tƒ }#t  |
¡}$|$d0k�r�|# !d1|$d.›d2�¡ t|||	dd�| }%|%d0k "¡ }&|&�rÐ|# !|&› d3t#|&ƒ› d4�¡ d5 $|#¡}#|#�rìd(|#› d6�}#t%|#ƒ }'t &tjj|d	d�d!¡�st‚t d7|d.›d+|"d.›d8�¡ t d9|'�r@d:nd;› d<|#› �¡ |j't|ƒdfk�sht‚|	j't|ƒd	fk�s€t‚t t|ƒ¡}(|	d=d=…df |(|< ||(|'fS )>z<Adjust coil normals using magnetometers and empty-room data.r   )Úfmin_cobylaç      Y@r&   r%   r(   Tr'   r   )r#   ©Ú	int_orderr"   rY   ©Z
ignore_ref©Z
coil_scalec                 S   s   g | ]}|d  dd… ‘qS )r/   r6   Nr'   r0   r'   r'   r3   r4   ð   s     z'_adjust_mag_normals.<locals>.<listcomp>r5   r-   )r:   Zkeepdimsc                 S   s   g | ]}|d  ‘qS r.   r'   r0   r'   r'   r3   r4   ô   s     c                   S   s   dS )Nr   r'   r'   r'   r'   r3   Ú<lambda>÷   ó    z%_adjust_mag_normals.<locals>.<lambda>)zworst firstzin orderz(        Magnetometer normal adjustment (z) ...r9   r   g      @)	rt   Ú	all_coilsÚcal_idxrc   r=   Ú	match_idxÚS_totrY   r"   gü©ñÒMbP?rq   F)ZconsZrhobegZrhoendr$   rv   )Úinplace©r`   r+   z        Optimization step Z3du    ï½œ z (u
   ) ï½œ res z5.2fu   â†’u   % ï½œ Ã—z0.3fz0.2fõ   Â°g      @z
max angle u    >= 5Â°z	 residualz > 5%z, ú)z        Fit mismatch ú%z        Data segment Ú ÚunÚusableN)(Zscipy.optimizer|   r   r   rD   rE   rF   rV   rC   r   r   Ú	_data_errrH   rr   rs   rJ   rS   r   rK   r   rA   rM   ZargsortÚpoprW   r   Ú_cal_sss_targetÚ_rotate_coilrw   Zrad2degr{   ÚdebugÚmaxrQ   rx   r   ÚjoinÚboolrm   Úshape))rA   rc   rY   r"   r|   Ú	mag_scaleZ	picks_useZ	picks_megZpicks_mag_origr=   ZanglesZ	picks_magÚexpr„   r‡   Z	first_errr\   r]   Zorig_zsr†   r8   r_   ÚcountsZkiÚkindÚerrÚorderr…   rt   Z	objectiver`   Zold_errZnew_errÚch_nameZlast_errÚreasonZ	max_angleZeach_errZn_badrf   r7   r'   r'   r3   rP   Ú   sÆ    


       ý    þ


 ÿHÿ



 rP   c                 C   sp   |d krt d ƒ}|| }t t || t| ¡ ƒd ¡| ¡}dtjj|| |  |d�tjj| | |d�  }|S )Nr   éd   r9   )ÚslicerD   rO   r   rJ   rr   rs   )rc   r‡   r=   r`   r:   Z
data_modelr�   r'   r'   r3   r�   E  s     ÿÿr�   Fc           
      C   s²   |t j |¡ }| t j | ¡ } t|| ƒ}|d | }t  ||d | j¡j}t  ||d | j¡j}|r‚||d |< ||d |< ||t  |jd t¡d|d |g d|if}	|	S )zAdjust coils.é   r   r   é   )	rD   rr   rs   r   rO   ÚTrV   r˜   rI   )
rg   rt   r„   r`   rˆ   ru   Zthis_slZ	this_rmagZthis_cosmagZsubsetr'   r'   r3   r“   P  s    
  ÿr“   c
                 C   sd   dt j | ¡ ||< td||d�}
| ¡ }|	| D ] }t| |||ƒ}t|
|ƒ||< q2t||||d�S )zCEvaluate objective function for SSS-based magnetometer calibration.rv   r   r~   r‰   )rD   rr   rs   rC   rJ   r“   r   r�   )rg   rt   r„   r…   rc   r=   r‡   rY   r"   r†   rš   r`   Z	this_coilr'   r'   r3   r’   a  s    r’   c                    sÎ  d}d}t | ddd�}t | ddd�}	| ¡ }||dd…f  |9  < ~t t|	ƒ|f¡}
t|d|d	�‰ t| d
d�}t| |d
d�}tˆ ||d�}||dd…tj	f  }t 
|gt¡‰t 
‡ ‡fdd„|D ƒ¡}t|ƒD �]Ø}| ¡ }||	 }||	dd…f  t d|
j|¡7  < t t| ¡ ƒd |¡}|
 ¡ }t ||¡}|j|t|	ƒ|jd fk�sZt‚||  ddd¡}|jt|	ƒ|jd |fk�sŒt‚||	 | }|jt|	ƒ|jd fk�s¶t‚|dd…tj	dd…f | dd…df }|jt|	ƒ|fk�söt‚t| dd¡| dd�\}}}|jt|	ƒ||fk�s.t‚|dd…tj	f | dd…df |
dd…< tj |
| ¡ttj |
¡tj |¡ƒ }t d|d › d|› dd| ¡  d›d�¡ qÔt t|ƒ|f¡}|
||	< |S )z*Estimate gradiometer imbalance parameters.r}   r   r&   r'   r(   r*   Nr   )rY   r   r"   Tr€   r�   c                    s   g | ]}t ˆ |d ˆƒ‘qS ©N)r   )r1   Zcoils©rš   Zthis_csr'   r3   r4   ƒ  s   ÿz'_estimate_imbalance.<locals>.<listcomp>z
ij,ijk->jkr   r   r-   éþÿÿÿg�íµ ÷Æ°>)Zrcondz        Iteration ú/u   : max âˆ† = r¡   z7.3frŒ   )r   rJ   rD   rV   rF   rC   r   r   r   ZnewaxisrH   ÚfloatrX   Zeinsumr¥   rO   r   r˜   rW   Z	transposer   Zswapaxesrr   rs   r•   r   r”   )rA   rc   r=   r    rY   r"   r™   Zn_iterationsr?   rZ   Zgrad_imbr„   Zgrad_point_coilsZS_origZS_ptÚkr‡   ZS_reconÚmmZprev_imbZ
data_reconZkhi_ptsZresidualÚdÚdinvr[   Zdeltasr>   r'   r§   r3   rU   p  sZ      ÿÿ$  (*ÿ.rU   c              
   C   s"  t | ddd�} t| ddƒ tƒ tƒ tƒ   }}}t| dƒ�Ê}|D ]¾}|d dkrRq@| ¡  ¡ }t|ƒd	kr~td
t|ƒ|f ƒ‚|d }t|ƒdkrÀzt|ƒ}W n t	k
r²   Y nX dd|  }| 
|¡ | 
t |dd… t¡¡ | 
t |dd… t¡¡ q@W 5 Q R X t |¡}t|||d�S )a�  Read fine calibration information from a .dat file.

    The fine calibration typically includes improved sensor locations,
    calibration coefficients, and gradiometer imbalance information.

    Parameters
    ----------
    fname : str
        The filename.

    Returns
    -------
    calibration : dict
        Fine calibration information. Key-value pairs are:

        - ``ch_names``
             List of str of the channel names.
        - ``locs``
             Coil location and orientation parameters.
        - ``imb_cals``
             For magnetometers, the calibration coefficients.
             For gradiometers, one or three imbalance parameters.
    ÚreadT)Ú	overwriteZ
must_existre   ©z.datÚrr   z#
)é   é   zcError parsing fine calibration file, should have 14 or 16 entries per line but found %s on line:
%s)r   r¤   ZMEGz%04dr   é   Nr@   )r   r   rM   ÚopenÚstripÚsplitrF   rR   rI   rB   rQ   rD   rH   rª   rC   )Úfnamer+   r8   r7   ZfidÚlineÚvalsrŸ   r'   r'   r3   Úread_fine_calibration«  s0    
ý
&
r¼   c              	      sœ   t | dd�} t| ddƒ d}t| dƒ�l}t‡ fdd„|D ƒŽ D ]N\}}}t ||g¡ d	¡}d
 dd„ |D ƒ¡}| |› d
|› d� 	d¡¡ q>W 5 Q R X dS )zÆWrite fine calibration information to a .dat file.

    Parameters
    ----------
    fname : str
        The filename to write out.
    calibration : dict
        Fine calibration information.
    T)r°   re   r±   r@   Úwbc                 3   s   | ]}ˆ | V  qd S r¦   r'   )r1   Úkey©r,   r'   r3   Ú	<genexpr>ñ  s     z)write_fine_calibration.<locals>.<genexpr>é   ú c                 s   s   | ]}|d ›V  qdS )z0.6fNr'   )r1   Úcr'   r'   r3   rÀ   ó  s     Ú
ÚASCIIN)
r   r   r¶   rN   rD   ZconcatenateÚroundr–   ÚwriteÚencode)r¹   r,   ÚkeysZcal_filerŸ   r/   Zimb_calZcal_liner'   r¿   r3   Úwrite_fine_calibrationã  s    
 rÊ   )r   r   r   r   NNN)NN)F)-Úcollectionsr   Ú	functoolsr   ÚnumpyrD   Zbemr   Zio.pickr   r   Úior   r	   r
   Z
transformsr   Úutilsr   r   r   r   r   r   r   r   r   Zmaxwellr   r   r   r   r   r   rl   rS   rT   r{   rP   r�   r“   r’   rU   r¼   rÊ   r'   r'   r'   r3   Ú<module>   s4   ,        þ *k

;8