#!/usr/bin/env python
import errno
import glob
import json
import os
import select
import termios
from collections import deque
from datetime import datetime

import rospy
from sensor_msgs.msg import NavSatFix, NavSatStatus
from std_msgs.msg import String


PORT = 'auto'
BAUD = 4800
FRAME_ID = 'gps'
READ_SIZE = 512

BAUD_MAP = {
    4800: termios.B4800,
    9600: termios.B9600,
    19200: termios.B19200,
    38400: termios.B38400,
    57600: termios.B57600,
    115200: termios.B115200,
}


def nmea_to_decimal(raw_value, hemisphere):
    if not raw_value or not hemisphere:
        return None

    try:
        value = float(raw_value)
    except ValueError:
        return None

    degrees = int(value / 100)
    minutes = value - degrees * 100
    decimal = degrees + minutes / 60.0

    if hemisphere in ('S', 'W'):
        decimal *= -1.0

    return decimal


def verify_checksum(sentence):
    if not sentence.startswith('$'):
        return False

    if '*' not in sentence:
        return True

    payload, checksum_text = sentence[1:].split('*', 1)
    checksum_text = checksum_text.strip()
    checksum = 0
    for char in payload:
        checksum ^= ord(char)

    try:
        expected = int(checksum_text[:2], 16)
    except ValueError:
        return False

    return checksum == expected


class GpsState(object):
    def __init__(self):
        self.connected = False
        self.latitude = None
        self.longitude = None
        self.altitude = None
        self.hdop = None
        self.speed_mps = None
        self.track_deg = None
        self.fix_quality = 0
        self.satellites = 0
        self.pdop = None
        self.vdop = None
        self.valid = False
        self.last_sentence_time = None
        self.last_fix_time = None
        self.utc_datetime = None
        self.visible_satellites = {}
        self.visible_satellite_count = 0
        self._gsv_groups = {}
        self.raw_sentences = deque(maxlen=200)

    def summary_text(self):
        if not self.connected:
            return 'Offline'
        if self.valid and self.latitude is not None and self.longitude is not None:
            speed_kmh = (self.speed_mps or 0.0) * 3.6
            return 'Fix | {0} Sat | {1:.1f} km/h'.format(max(0, int(self.satellites or 0)), speed_kmh)
        if self.last_sentence_time is not None:
            return 'Suche Satelliten'
        return 'Initialisiere'


def configure_serial(fd, baud):
    attrs = termios.tcgetattr(fd)
    attrs[0] = 0
    attrs[1] = 0
    attrs[2] = termios.CS8 | termios.CREAD | termios.CLOCAL
    attrs[3] = 0
    attrs[4] = baud
    attrs[5] = baud
    attrs[6][termios.VMIN] = 0
    attrs[6][termios.VTIME] = 0
    termios.tcsetattr(fd, termios.TCSANOW, attrs)
    termios.tcflush(fd, termios.TCIFLUSH)


def resolve_port_candidates(port_config):
    if port_config is None:
        port_config = PORT

    config_text = str(port_config).strip()
    if not config_text or config_text.lower() == 'auto':
        raw_candidates = [
            '/dev/serial/by-id/*',
            '/dev/serial/by-path/*',
            '/dev/ttyUSB*',
            '/dev/ttyACM*',
        ]
    else:
        raw_candidates = [item.strip() for item in config_text.split(',') if item.strip()]

    candidates = []
    seen = set()
    for candidate in raw_candidates:
        matches = sorted(glob.glob(candidate)) if any(char in candidate for char in '*?[') else [candidate]
        for match in matches:
            if match in seen:
                continue
            seen.add(match)
            candidates.append(match)
    return candidates


def open_device(port_config, baud, baud_rate):
    last_signature = None
    while not rospy.is_shutdown():
        candidates = resolve_port_candidates(port_config)
        signature = tuple(candidates)
        if not candidates:
            if signature != last_signature:
                rospy.loginfo('GPS wartet auf Geraet. Suchmuster: %s', port_config)
                last_signature = signature
            rospy.sleep(1.0)
            continue

        try:
            for port in candidates:
                try:
                    fd = os.open(port, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK)
                    configure_serial(fd, baud)
                    rospy.loginfo('GPS verbunden: %s @ %s Baud', port, baud_rate)
                    return fd
                except OSError as exc:
                    if exc.errno != errno.ENOENT:
                        rospy.logwarn('GPS kann nicht geoeffnet werden (%s): %s', port, exc)
                except termios.error as exc:
                    rospy.logwarn('GPS-Port kann nicht konfiguriert werden (%s): %s', port, exc)
        finally:
            last_signature = signature
        rospy.sleep(1.0)
    return None


def close_device(fd):
    if fd is None:
        return
    try:
        os.close(fd)
    except OSError:
        pass


def parse_gga(fields, state):
    if len(fields) < 10:
        return False

    latitude = nmea_to_decimal(fields[2], fields[3])
    longitude = nmea_to_decimal(fields[4], fields[5])

    try:
        fix_quality = int(fields[6] or 0)
    except ValueError:
        fix_quality = 0

    try:
        satellites = int(fields[7] or 0)
    except ValueError:
        satellites = 0

    try:
        hdop = float(fields[8]) if fields[8] else None
    except ValueError:
        hdop = None

    try:
        altitude = float(fields[9]) if fields[9] else None
    except ValueError:
        altitude = None

    if latitude is not None:
        state.latitude = latitude
    if longitude is not None:
        state.longitude = longitude
    state.fix_quality = fix_quality
    state.satellites = satellites
    state.hdop = hdop
    state.altitude = altitude
    state.valid = fix_quality > 0 and latitude is not None and longitude is not None
    if state.valid:
        state.last_fix_time = rospy.Time.now()
    return True


def parse_rmc(fields, state):
    if len(fields) < 10:
        return False

    status = fields[2] if len(fields) > 2 else ''
    latitude = nmea_to_decimal(fields[3], fields[4])
    longitude = nmea_to_decimal(fields[5], fields[6])

    try:
        speed_knots = float(fields[7]) if fields[7] else 0.0
    except ValueError:
        speed_knots = 0.0

    try:
        track_deg = float(fields[8]) if fields[8] else 0.0
    except ValueError:
        track_deg = 0.0

    if latitude is not None:
        state.latitude = latitude
    if longitude is not None:
        state.longitude = longitude
    state.speed_mps = speed_knots * 0.514444
    state.track_deg = track_deg
    if status == 'A' and state.latitude is not None and state.longitude is not None:
        state.valid = True
        state.last_fix_time = rospy.Time.now()

    if fields[1] and fields[9]:
        try:
            state.utc_datetime = datetime.strptime(fields[9] + fields[1].split('.')[0], '%d%m%y%H%M%S')
        except ValueError:
            pass
    return True


def parse_gsa(fields, state):
    if len(fields) < 18:
        return False

    try:
        state.pdop = float(fields[15]) if fields[15] else None
    except ValueError:
        state.pdop = None

    try:
        state.hdop = float(fields[16]) if fields[16] else state.hdop
    except ValueError:
        pass

    try:
        state.vdop = float(fields[17]) if fields[17] else None
    except ValueError:
        state.vdop = None
    return True


def merge_visible_satellites(state):
    now = rospy.Time.now()
    merged = {}

    stale_talkers = []
    for talker, group in state._gsv_groups.items():
        completed_at = group.get('completed_at')
        if completed_at is None:
            continue
        try:
            if (now - completed_at).to_sec() > 8.0:
                stale_talkers.append(talker)
                continue
        except rospy.ROSException:
            continue

        constellation = group.get('constellation', talker)
        for sat_key, satellite in group.get('satellites', {}).items():
            merged[sat_key] = dict(satellite, constellation=constellation)

    for talker in stale_talkers:
        state._gsv_groups.pop(talker, None)

    state.visible_satellites = dict(sorted(merged.items()))
    state.visible_satellite_count = len(state.visible_satellites)


def parse_gsv(fields, state, sentence_type):
    if len(fields) < 4:
        return False

    try:
        total_sentences = int(fields[1] or 0)
        sentence_index = int(fields[2] or 0)
        total_satellites = int(fields[3] or 0)
    except ValueError:
        return False

    if sentence_index <= 0:
        return False

    talker = sentence_type[1:3] if len(sentence_type) >= 3 else 'GN'
    group = state._gsv_groups.get(talker)
    if group is None:
        group = {
            'constellation': talker,
            'total_sentences': 0,
            'working_set': {},
            'satellites': {},
            'completed_at': None,
            'total_satellites_reported': 0,
        }
        state._gsv_groups[talker] = group

    if sentence_index == 1 or total_sentences != group.get('total_sentences'):
        group['working_set'] = {}

    group['total_sentences'] = total_sentences
    group['total_satellites_reported'] = total_satellites

    for index in range(4, len(fields), 4):
        if index + 3 >= len(fields):
            break
        prn = fields[index].strip()
        if not prn:
            continue
        try:
            elevation = int(fields[index + 1]) if fields[index + 1] else None
        except ValueError:
            elevation = None
        try:
            azimuth = int(fields[index + 2]) if fields[index + 2] else None
        except ValueError:
            azimuth = None
        try:
            snr = int(fields[index + 3]) if fields[index + 3] else None
        except ValueError:
            snr = None

        satellite_key = '{0}:{1}'.format(talker, prn)
        group['working_set'][satellite_key] = {
            'prn': '{0}-{1}'.format(talker, prn),
            'elevation': elevation,
            'azimuth': azimuth,
            'snr': snr
        }

    if sentence_index >= total_sentences:
        group['satellites'] = dict(group['working_set'])
        group['completed_at'] = rospy.Time.now()
        merge_visible_satellites(state)
    return True


def parse_sentence(sentence, state):
    if not verify_checksum(sentence):
        return False

    state.raw_sentences.append(sentence)

    fields = sentence.split('*', 1)[0].split(',')
    if not fields:
        return False

    sentence_type = fields[0]
    state.last_sentence_time = rospy.Time.now()

    if sentence_type.endswith('GGA'):
        return parse_gga(fields, state)
    if sentence_type.endswith('RMC'):
        return parse_rmc(fields, state)
    if sentence_type.endswith('GSA'):
        return parse_gsa(fields, state)
    if sentence_type.endswith('GSV'):
        return parse_gsv(fields, state, sentence_type)
    return False


def build_diagnostics_payload(state, frame_id):
    last_fix_age = None
    if state.last_fix_time is not None:
        try:
            last_fix_age = max(0.0, (rospy.Time.now() - state.last_fix_time).to_sec())
        except rospy.ROSException:
            last_fix_age = None

    return {
        'connected': state.connected,
        'frame_id': frame_id,
        'valid': state.valid,
        'fix_quality': state.fix_quality,
        'satellites_used': max(0, int(state.satellites or 0)),
        'satellites_visible': max(0, int(state.visible_satellite_count or 0)),
        'latitude': state.latitude,
        'longitude': state.longitude,
        'altitude_m': state.altitude,
        'hdop': state.hdop,
        'pdop': state.pdop,
        'vdop': state.vdop,
        'speed_mps': state.speed_mps,
        'speed_kmh': (state.speed_mps or 0.0) * 3.6 if state.speed_mps is not None else None,
        'track_deg': state.track_deg,
        'utc_time': state.utc_datetime.strftime('%H:%M:%S') if state.utc_datetime else None,
        'utc_date': state.utc_datetime.strftime('%Y-%m-%d') if state.utc_datetime else None,
        'last_fix_age_s': last_fix_age,
        'satellites': list(state.visible_satellites.values()),
        'raw_sentences': list(state.raw_sentences)
    }


def publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id):
    message = NavSatFix()
    message.header.stamp = rospy.Time.now()
    message.header.frame_id = frame_id
    message.status.service = NavSatStatus.SERVICE_GPS

    if state.valid and state.latitude is not None and state.longitude is not None:
        message.status.status = NavSatStatus.STATUS_FIX
        message.latitude = state.latitude
        message.longitude = state.longitude
        message.altitude = state.altitude if state.altitude is not None else 0.0
        if state.hdop is not None:
            horizontal_variance = max(0.25, state.hdop * state.hdop)
            vertical_variance = max(1.0, (state.hdop * 2.0) * (state.hdop * 2.0))
            message.position_covariance = [
                horizontal_variance, 0.0, 0.0,
                0.0, horizontal_variance, 0.0,
                0.0, 0.0, vertical_variance
            ]
            message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
        else:
            message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
    else:
        message.status.status = NavSatStatus.STATUS_NO_FIX
        message.latitude = 0.0
        message.longitude = 0.0
        message.altitude = 0.0
        message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN

    fix_publisher.publish(message)
    overlay_publisher.publish(String(data=state.summary_text()))
    diagnostics_publisher.publish(String(data=json.dumps(build_diagnostics_payload(state, frame_id), sort_keys=True)))


if __name__ == '__main__':
    rospy.init_node('gps_node')

    port = rospy.get_param('~port', PORT)
    baud_rate = int(rospy.get_param('~baud', BAUD))
    frame_id = rospy.get_param('~frame_id', FRAME_ID)
    baud = BAUD_MAP.get(baud_rate, termios.B4800)

    fix_publisher = rospy.Publisher('/fix', NavSatFix, queue_size=5)
    overlay_publisher = rospy.Publisher('/gps/status_text', String, queue_size=5)
    diagnostics_publisher = rospy.Publisher('/gps/diagnostics_json', String, queue_size=5)

    state = GpsState()
    serial_fd = None
    buffer = bytearray()

    while not rospy.is_shutdown():
        if serial_fd is None:
            serial_fd = open_device(port, baud, baud_rate)
            buffer = bytearray()
            state.connected = serial_fd is not None
            publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
            if serial_fd is None:
                break

        try:
            readable, _, _ = select.select([serial_fd], [], [], 0.5)
            if serial_fd not in readable:
                publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
                continue

            chunk = os.read(serial_fd, READ_SIZE)
            if not chunk:
                raise OSError(errno.ENODEV, 'GPS getrennt')

            buffer.extend(chunk)

            while b'\n' in buffer:
                raw_line, _, buffer = buffer.partition(b'\n')
                line = raw_line.decode('ascii', 'ignore').strip()
                if not line or not line.startswith('$'):
                    continue
                if parse_sentence(line, state):
                    publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)

        except OSError as exc:
            if exc.errno not in (errno.EAGAIN, errno.EWOULDBLOCK):
                rospy.logwarn('GPS-Lesefehler: %s', exc)
                close_device(serial_fd)
                serial_fd = None
                state.connected = False
                state.valid = False
                state.last_sentence_time = None
                state.satellites = 0
                rospy.sleep(1.0)
        except termios.error as exc:
            rospy.logwarn('GPS-Fehler: %s', exc)
            close_device(serial_fd)
            serial_fd = None
            state.connected = False
            state.valid = False
            state.last_sentence_time = None
            state.satellites = 0
            rospy.sleep(1.0)

    close_device(serial_fd)