#!/usr/bin/env python3
from __future__ import absolute_import, division, print_function

import curses
import sys
from collections import deque
from datetime import datetime

import numpy as np
import rospy
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import ExtendedState, PositionTarget, State  # StatusText
from scipy.spatial.transform import Rotation as R
from sensor_msgs.msg import BatteryState, Image, NavSatFix

GPS_FIX_DICT = {
    0: ('No GPS', curses.COLOR_RED),
    1: ('No fix', curses.COLOR_RED),
    2: ('2D lock', curses.COLOR_BLUE),
    3: ('3D lock', curses.COLOR_BLUE),
    4: ('DGPS', curses.COLOR_MAGENTA),
    5: ('RTK float', curses.COLOR_YELLOW),
    6: ('RTK fix', curses.COLOR_GREEN)
}


def get_color(color):
    return curses.color_pair(color)


def frequency_from_messages(messages):
    durations = []
    for i in range(len(messages) - 1):
        duration = messages[i + 1].header.stamp - messages[i].header.stamp
        durations.append(duration.to_sec())
    frequency = 1 / np.mean(durations)
    if 
np.isnan(frequency)