""" Functions to import data from Invesalius """

import transformations as tr
import pandas as pd
import numpy as np
import datetime


def read_triggermarker(fn_mkss):
    """
    Read instrument-marker and conditions from neuronavigator .xml-file.

    Parameters
    ----------
    fn_mkss : str
        Path to TriggerMarker.fn_mkss file

    Returns
    -------
    m_nnav : np.ndarray of float
        (4, 4, N) Neuronavigation coordinates of N stimuli (4x4 matsimnibs matrices).
    didt : np.ndarray of float
        (N) Rate of change of coil current in (A/us). At the moment: NAN.
    stim_int : np.ndarray of float
        (N) Stimulator intensity in (% MSO). At the moment: NAN.
    descr : list of str
        (N) Labels of the instrument-marker-conditions.
    rec_time : list of str
        (N) Recording time in ms.
    """
    data = pd.read_csv(fn_mkss, skiprows=1, sep="\t")
    data = data[data['marker_type'] == 4]
    if 'is_tracked' not in data.columns:
        print("is_tracked column not found, assuming all markers are tracked")
        tracking_missing = True
    else:
        tracking_missing = False
        print(f"{data[data['is_tracked'] == 0].shape[0]} untracked markers found.")
        # data = data[data['is_tracked'] == 1]
    print(f"{data.shape[0]} coil placements found in {fn_mkss}")

    # build matsimnibs from data
    matsimnibs = []
    desc = []
    rec_time = []
    start_time = data['timestamp'].iloc[0]
    dt_format = "%Y-%m-%d %H:%M:%S.%f"
    start_time = datetime.datetime.strptime(start_time, dt_format)
    for idx, row in data.iterrows():
        if not tracking_missing and row[data['is_tracked'] == 0]:
            print(f"Skipping untracked marker {idx}")
            r = np.eye(4)
            r[:3, :4] = np.nan
        r = tr.euler_matrix(row["alpha_world"], row["beta_world"], row["gamma_world"], "sxyz")
        r[:3,3] = row[["x_world", "y_world", "z_world"]].values
        matsimnibs.append(r)

        desc.append(row["marker_id"])
        rec_time.append((datetime.datetime.strptime(row["timestamp"], dt_format) - start_time).total_seconds() * 1000)
    matsimnibs = np.array(matsimnibs)
    matsimnibs = np.moveaxis(matsimnibs, 0, -1)

    didt, stim_int, descr = (np.zeros(matsimnibs.shape[-1]),
                             np.zeros(matsimnibs.shape[-1]),
                             np.zeros(matsimnibs.shape[-1]))

    # currently not storted by Invesalius
    didt[:] = np.nan
    stim_int[:] = np.nan
    return matsimnibs, didt, stim_int, desc, rec_time
