"""Create coordinate transforms."""

# Author: Eric Larson <larson.eric.d<gmail.com>
#
# License: BSD (3-clause)

import numpy as np

from ...transforms import (combine_transforms, invert_transform, Transform,
                           _quat_to_affine, _fit_matched_points, apply_trans,
                           get_ras_to_neuromag_trans)
from ...utils import logger
from ..constants import FIFF
from .constants import CTF


def _make_transform_card(fro, to, r_lpa, r_nasion, r_rpa):
    """Make a transform from cardinal landmarks."""
    return invert_transform(Transform(
        to, fro, get_ras_to_neuromag_trans(r_nasion, r_lpa, r_rpa)))


def _quaternion_align(from_frame, to_frame, from_pts, to_pts, diff_tol=1e-4):
    """Perform an alignment using the unit quaternions (modifies points)."""
    assert from_pts.shape[1] == to_pts.shape[1] == 3
    trans = _quat_to_affine(_fit_matched_points(from_pts, to_pts)[0])

    # Test the transformation and print the results
    logger.info('    Quaternion matching (desired vs. transformed):')
    for fro, to in zip(from_pts, to_pts):
        rr = apply_trans(trans, fro)
        diff = np.linalg.norm(to - rr)
        logger.info('    %7.2f %7.2f %7.2f mm <-> %7.2f %7.2f %7.2f mm '
                    '(orig : %7.2f %7.2f %7.2f mm) diff = %8.3f mm'
                    % (tuple(1000 * to) + tuple(1000 * rr) +
                       tuple(1000 * fro) + (1000 * diff,)))
        if diff > diff_tol:
            raise RuntimeError('Something is wrong: quaternion matching did '
                               'not work (see above)')
    return Transform(from_frame, to_frame, trans)


def _make_ctf_coord_trans_set(res4, coils):
    """Figure out the necessary coordinate transforms."""
    # CTF head > Neuromag head
    lpa = rpa = nas = T1 = T2 = T3 = T5 = None
    if coils is not None:
        for p in coils:
            if p['valid'] and (p['coord_frame'] ==
                               FIFF.FIFFV_MNE_COORD_CTF_HEAD):
                if lpa is None and p['kind'] == CTF.CTFV_COIL_LPA:
                    lpa = p
                elif rpa is None and p['kind'] == CTF.CTFV_COIL_RPA:
                    rpa = p
                elif nas is None and p['kind'] == CTF.CTFV_COIL_NAS:
                    nas = p
        if lpa is None or rpa is None or nas is None:
            raise RuntimeError('Some of the mandatory HPI device-coordinate '
                               'info was not there.')
        t = _make_transform_card('head', 'ctf_head',
                                 lpa['r'], nas['r'], rpa['r'])
        T3 = invert_transform(t)

    # CTF device -> Neuromag device
    #
    # Rotate the CTF coordinate frame by 45 degrees and shift by 190 mm
    # in z direction to get a coordinate system comparable to the Neuromag one
    #
    R = np.eye(4)
    R[:3, 3] = [0., 0., 0.19]
    val = 0.5 * np.sqrt(2.)
    R[0, 0] = val
    R[0, 1] = -val
    R[1, 0] = val
    R[1, 1] = val
    T4 = Transform('ctf_meg', 'meg', R)

    # CTF device -> CTF head
    # We need to make the implicit transform explicit!
    h_pts = dict()
    d_pts = dict()
    kinds = (CTF.CTFV_COIL_LPA, CTF.CTFV_COIL_RPA, CTF.CTFV_COIL_NAS,
             CTF.CTFV_COIL_SPARE)
    if coils is not None:
        for p in coils:
            if p['valid']:
                if p['coord_frame'] == FIFF.FIFFV_MNE_COORD_CTF_HEAD:
                    for kind in kinds:
                        if kind not in h_pts and p['kind'] == kind:
                            h_pts[kind] = p['r']
                elif p['coord_frame'] == FIFF.FIFFV_MNE_COORD_CTF_DEVICE:
                    for kind in kinds:
                        if kind not in d_pts and p['kind'] == kind:
                            d_pts[kind] = p['r']
        if any(kind not in h_pts for kind in kinds[:-1]):
            raise RuntimeError('Some of the mandatory HPI device-coordinate '
                               'info was not there.')
        if any(kind not in d_pts for kind in kinds[:-1]):
            raise RuntimeError('Some of the mandatory HPI head-coordinate '
                               'info was not there.')
        use_kinds = [kind for kind in kinds
                     if (kind in h_pts and kind in d_pts)]
        r_head = np.array([h_pts[kind] for kind in use_kinds])
        r_dev = np.array([d_pts[kind] for kind in use_kinds])
        T2 = _quaternion_align('ctf_meg', 'ctf_head', r_dev, r_head)

    # The final missing transform
    if T3 is not None and T2 is not None:
        T5 = combine_transforms(T2, T3, 'ctf_meg', 'head')
        T1 = combine_transforms(invert_transform(T4), T5, 'meg', 'head')
    s = dict(t_dev_head=T1, t_ctf_dev_ctf_head=T2, t_ctf_head_head=T3,
             t_ctf_dev_dev=T4, t_ctf_dev_head=T5)
    logger.info('    Coordinate transformations established.')
    return s
