#!/usr/bin/env python
# -*- coding: utf-8 -*-
import wx
import numpy as np
from math import sqrt

# ROS imports
import rospy
import std_msgs.msg
from sensor_msgs.msg import Image, CompressedImage, NavSatFix, Imu
from nav_msgs.msg import Odometry
from diagnostic_msgs.msg import DiagnosticStatus
from tf.transformations import euler_from_quaternion

# ADAPT imports.
from sensor_models.nav_conversions import llh_to_enu, ned_quat_to_enu_quat, \
    enu_quat_to_ned_quat, quat_std_to_ypr_std


class SpatialINSInterface(object):
    """GUI's convenience interface to the Advanced Navigation Spatial INS.

    """
    def __init__(self, nav_sat_fix_topic, imu_topic, nav_filter_status_topic,
                 ins_update_callback):
        """
        """
        self.nav_sat_fix_sub = rospy.Subscriber(nav_sat_fix_topic,
                                                NavSatFix,
                                                self.nav_sat_fix_ros,
                                                queue_size=1)

        self.imu_sub = rospy.Subscriber(imu_topic, Imu, self.imu_ros,
                                        queue_size=1)

        self.imu_sub = rospy.Subscriber(nav_filter_status_topic,
                                        DiagnosticStatus, self.status_msg_ros,
                                        queue_size=1)

        self.ins_update_callback = ins_update_callback

    def nav_sat_fix_ros(self, msg):
        """
        :param msg: INS message.
        :type msg: NavSatFix

        """
        wx.CallAfter(self.nav_sat_fix, msg)

    def nav_sat_fix(self, msg):
        """
        :param msg: INS message.
        :type msg: NavSatFix

        """
        t = msg.header.stamp.to_sec()
        lat = msg.latitude
        lon = msg.longitude
        h = msg.altitude
        cov = msg.position_covariance
        easting_std = np.sqrt(cov[0])
        northing_std = np.sqrt(cov[4])
        h_std = np.sqrt(cov[8])
        self.ins_update_callback(t=t, lat=lat, lon=lon, h=h,
                                 easting_std=easting_std,
                                 northing_std=northing_std, h_std=h_std)

    def imu_ros(self, msg):
        wx.CallAfter(self.imu, msg)

    def imu(self, msg):
        t = msg.header.stamp.to_sec()

        # NED quaternion

        ned_quat = np.array([msg.orientation.x, msg.orientation.y,
                             msg.orientation.z, msg.orientation.w])

        #ned_quat = enu_quat_to_ned_quat(quat)
        heading, pitch, roll = euler_from_quaternion(ned_quat, axes='rzyx')

        heading *= 180/np.pi
        pitch *= 180/np.pi
        roll *= 180/np.pi

        # Calculate heading, pitch, roll uncertainty.
        # Packet 27, Fields 2, 3, 4, Anpp Packet 27 Field 1 is for the W axis
        # which is not requested.
        qx_std = msg.orientation_covariance[0]
        qy_std = msg.orientation_covariance[4]
        qz_std = msg.orientation_covariance[8]

        heading_std, pitch_std, roll_std = quat_std_to_ypr_std(ned_quat,
                                                               qx_std,
                                                               qy_std,
                                                               qz_std)

        heading_std *= 180/np.pi
        pitch_std *= 180/np.pi
        roll_std *= 180/np.pi

        self.ins_update_callback(t=t, heading=heading, pitch=pitch, roll=roll,
                                 heading_std=heading_std, pitch_std=pitch_std,
                                 roll_std=roll_std)

    def status_msg_ros(self, msg):
        wx.CallAfter(self.status_msg, msg)

    def status_msg(self, msg):
        m = msg.message
        orientation_fiter_init = '0. Orientation Filter Initialised.' in m
        navigation_filter_init = '1. Navigation Filter Initialised.' in m
        heading_initialized = '2. Heading Initialised.' in m
        utc_time_initialized = '3. UTC Time Initialised.' in m
        internal_gnss_enabled = '9. Internal GNSS Enabled.' in m
        magnetic_heading = '10. Magnetic Heading Active.' in m
        velocity_heading = '11. Velocity Heading Enabled.' in m
        atmospheric_altitude = '12. Atmospheric Altitude Enabled.' in m

        self.ins_update_callback(orientation_fiter_init=orientation_fiter_init,
                                 navigation_filter_init=navigation_filter_init,
                                 heading_initialized=heading_initialized,
                                 utc_time_initialized=utc_time_initialized,
                                 internal_gnss_enabled=internal_gnss_enabled,
                                 magnetic_heading=magnetic_heading,
                                 velocity_heading=velocity_heading,
                                 atmospheric_altitude=atmospheric_altitude)

    def unregister(self):
        self.nav_sat_fix_sub.unregister()
        self.imu_sub.unregister()
