import argparse
import logging
import os
import sys
import spiceypy as cspice
import numpy as np
import subprocess


def setup_logging():
    logging.basicConfig(stream=sys.stdout, level=logging.INFO)


def assert_valid_matrix(row):
    m = as_rotation_matrix(row)
    assert (m.shape[0] == m.shape[1]) and np.allclose(m, np.eye(m.shape[0]))


def as_rotation_matrix(elements):
    return elements.reshape(3, 3)


def primary_to_inertial(row):
    return row[12:21].reshape(3, 3)


def secondary_to_primary(row):
    return row[21:30].reshape(3, 3)


def matrix_to_string(matrix):
    separator = " "
    return separator.join([str(component) for component in matrix.flatten()])


def dump_row(fd, timestamp, matrix):
    matrix_string = matrix_to_string(matrix)
    fd.write(f"{timestamp} {matrix_string}\n")


def launch_msopck(file_path):
    logging.info('Starting msopck')
    working_dir = os.path.join(
        os.path.dirname(__file__),
        'msopck'
    )
    cmd = os.path.join(
        working_dir,
        'dm_msopck'
    )
    ck_path = file_path.replace('.csv','.bc')
    process = subprocess.Popen([cmd, 'setup.msopck', file_path, ck_path],
                        stdout=subprocess.PIPE, 
                        stderr=subprocess.PIPE,
                        cwd=working_dir)
    stdout, stderr = process.communicate()

    if process.returncode == 0:
        logging.debug(stdout.decode())
        logging.info(f'CK generated: {ck_path}')
    else:
        logging.warning('Problems generating CK')
        logging.warning(stdout.decode())


if __name__ == "__main__":
    setup_logging()
    parser = argparse.ArgumentParser(
        description="Reader and CK generator for the dimorphos tumbling model"
    )
    parser.add_argument(
        "-t",
        "--time",
        help="Path of the binary file with time information t_out.bin",
        required=True,
    )
    parser.add_argument(
        "-d",
        "--data",
        help="Path of the binary file with position and rotation data x_out.bin",
        required=True,
    )
    parser.add_argument(
        "-m", "--metakernel", help="Path of the metakernel", required=True
    )
    parser.add_argument(
        "-o", "--output", help="Path of the CSV output file", required=True
    )

    parser.add_argument(
        "-i",
        "--impact_utc",
        help="Dart impact time utc",
        default="2022-09-26T23:14:24.183Z",
    )

    args = parser.parse_args()
    try: 
        # Setup
        cspice.furnsh(args.metakernel)

        time = np.fromfile(args.time)
        data = np.fromfile(args.data).reshape((-1, 30))

        dart_impact_utc = args.impact_utc
        dart_impact_et = cspice.utc2et(dart_impact_utc)
        logging.info(f"DART Impact Time {dart_impact_utc}")

        assert_valid_matrix(primary_to_inertial(data[0]))
        assert_valid_matrix(secondary_to_primary(data[0]))

        m_j2000_to_inertial = cspice.pxform("J2000", "DIDYMOS_FIXED", dart_impact_et)

        output_file_path = args.output + '.csv'
        with open(output_file_path, "w") as output:
            for index, row in enumerate(data):
                timestamp = cspice.et2utc(dart_impact_et + time[index], "ISOC", 3)
                m_didymos_to_inertial = primary_to_inertial(row)
                m_dimorphos_to_didymos = secondary_to_primary(row)
                m_j2000_to_dimorphos = cspice.mxm(
                    np.linalg.inv(m_dimorphos_to_didymos),
                    cspice.mxm(np.linalg.inv(m_didymos_to_inertial), m_j2000_to_inertial),
                )
                dump_row(output, timestamp, m_j2000_to_dimorphos)

        logging.info(f"Generated {output_file_path}")

        launch_msopck(os.path.abspath(output_file_path))
    except Exception as e:
        logging.error(e)