#!/usr/bin/env python
from __future__ import division, print_function
import os
import numpy as np

# ADAPT imports.
from postflight_scripts.ins import ANINS


# Bag information
nav_bag_fname = '/host_filesystem/mnt/homenas2/noaa_adapt/adapt_flights/2022-04-21/nav_data.bag'
odometry_txt_fname = '%s/odometry.txt' % os.path.split(nav_bag_fname)[0]
imu_txt_fname = '%s/imu.txt' % os.path.split(nav_bag_fname)[0]


ins = ANINS.from_ros_bag(nav_bag_fname)


# We take the INS-reported position (converted from latitude, longitude, and
# altitude into easting/northing/up coordinates) and assign it to each image.
ins.print_origin()

odometry = []
for t in ins.odometry_times:
    pos, quat, cov = ins.pose(t, in_enu=False, include_cov=True)
    std = np.sqrt(np.diag(cov))
    odometry.append([t, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2],
                     quat[3], std[0], std[1], std[2], std[3], std[4], std[5]])

odometry = np.array(odometry)

header = ('time (s)  latitude (deg)  longitude (deg)  height (m)  quatx  '
          'quaty  quatz  quatw std_easting (m) std_northing (m) '
          'std_up (m) std_heading (rad) std_pitch (rad) std_roll (rad)')
np.savetxt(odometry_txt_fname, odometry, header=header)


if False:
    data = ins.get_raw_imu().T
    header = ('time (s)  accel-x (m/s^2), accel-y (m/s^2), accel-z (m/s^2), '
              'gyro-x (rad/s), gyro-y (rad/s), gyro-z (rad/s), mag-x (Ga), '
              'mag-y (Ga), mag-z (Ga), imu-temp (C), pressure (Pa), '
              'ambient-temp (C)')
    np.savetxt(imu_txt_fname, data, header=header)

data = ins.get_imu().T
header = ('time (s)  accel-x (m/s^2), accel-y (m/s^2), accel-z (m/s^2), '
          'angular-x (rad/s), angular-y (rad/s), angular-z (rad/s)')
np.savetxt(imu_txt_fname, data, header=header)
