#!/usr/bin/python3
# -*- coding: utf-8 -*-

#
# Copyright (C) 2016 All Right Reserved, Southwest Research Institute® (SwRI®)
#

import fnmatch
import math
import os
from collections import namedtuple

import pyproj
import rclpy
import yaml
from geometry_msgs.msg import PoseStamped
from gps_msgs.msg import GPSFix
from mapviz_interfaces.srv import AddMapvizDisplay
from marti_common_msgs.msg import KeyValue
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile

GeoReference = namedtuple("GeoReference", "path min_lat min_lon max_lat max_lon area")


def distance(lat1, lon1, lat2, lon2):
    x1 = math.radians(lat1)
    y1 = math.radians(lon1)
    x2 = math.radians(lat2)
    y2 = math.radians(lon2)
    return math.acos(math.sin(x1) * math.sin(x2) + math.cos(x1) * math.cos(x2) * math.cos(y1 - y2))


class MapvizTileLoader(Node):
    def __init__(self):
        super().__init__('mapviz_tile_loader')

        self.base_directory = self.declare_parameter(
            'base_directory', os.path.expanduser('~') + "/.ros").value
        self.max_search_depth = self.declare_parameter('max_search_depth', 1).value
        rate = max(0.1, self.declare_parameter('rate', 1.0).value)
        self.display_name = self.declare_parameter('display_name', 'satellite').value
        self.draw_order = self.declare_parameter('draw_order', 1).value
        self.use_local_xy = self.declare_parameter('use_local_xy', False).value

        self._gps_fix = None
        self._local_xy = None
        self._last_path = None
        self._request_pending = False

        self._gps_sub = self.create_subscription(GPSFix, "gps", self.gps_callback, 10)

        if self.use_local_xy:
            # The origin is published latched, so subscribe with transient-local
            # durability to receive the last value sent before we subscribed.
            origin_qos = QoSProfile(
                depth=1,
                history=HistoryPolicy.KEEP_LAST,
                durability=DurabilityPolicy.TRANSIENT_LOCAL)
            self._local_xy_sub = self.create_subscription(
                PoseStamped, "/local_xy_origin", self.local_xy_callback, origin_qos)

        self.georeferences = self.load_georeferences()

        self._client = self.create_client(AddMapvizDisplay, 'add_mapviz_display')
        self.get_logger().info(f"waiting for service: {self._client.srv_name} ...")
        self._client.wait_for_service()

        self._timer = self.create_timer(1.0 / rate, self.on_timer)

    def gps_callback(self, data):
        self._gps_fix = data

    def local_xy_callback(self, data):
        self._local_xy = data

    def load_georeferences(self):
        geofiles = []
        initial_depth = self.base_directory.count(os.sep)
        for path, dirs, filenames in os.walk(self.base_directory):
            for filename in fnmatch.filter(filenames, '*.geo'):
                geofiles.append(os.path.join(path, filename))
            current_depth = path.count(os.sep) - initial_depth
            if current_depth >= self.max_search_depth:
                dirs[:] = []

        georeferences = []
        for geofile in geofiles:
            georef = self.parse_georeference(geofile)
            if georef is not None:
                georeferences.append(georef)
        return georeferences

    def parse_georeference(self, geofile):
        self.get_logger().info(f"Parsing {geofile}...")
        min_lat = 90.0
        max_lat = -90.0
        min_lon = 180.0
        max_lon = -180.0
        with open(geofile) as f:
            geodata = yaml.safe_load(f)
        if str(geodata['projection']).lower() == 'utm':
            self.get_logger().info("  projection: utm")
            zone = 1
            band = 'N'
            if 'utm_zone' in geodata:
                zone = int(geodata['utm_zone'])
                if zone >= 1 or zone <= 60:
                    self.get_logger().info(f"  utm zone: {zone}")
                else:
                    self.get_logger().warn("  invalid utm zone!")
                    return None
            else:
                self.get_logger().warn("  no utm zone!")
                return None
            if 'utm_band' in geodata:
                band = str(geodata['utm_band'])
                if band >= 'C' or band <= 'X':
                    self.get_logger().info(f"  utm band: {band}")
                else:
                    self.get_logger().warn("  invalid utm band!")
                    return None
            else:
                self.get_logger().warn("  no utm band!")
                return None

            utm_proj = pyproj.Proj(proj='utm', zone=zone, ellps='WGS84', south=(band < 'N'))

            if 'tiepoints' in geodata:
                if len(geodata['tiepoints']) > 1:
                    for point in geodata['tiepoints']:
                        easting = point['point'][2]
                        northing = point['point'][3]
                        lon, lat = utm_proj(easting, northing, inverse=True)
                        min_lon = min(lon, min_lon)
                        max_lon = max(lon, max_lon)
                        min_lat = min(lat, min_lat)
                        max_lat = max(lat, max_lat)
                else:
                    self.get_logger().warn("  not enough tiepoints!")
                    return None
            else:
                self.get_logger().warn("  no tiepoints!")
                return None

        area = (max_lat - min_lat) * (max_lon - min_lon)

        self.get_logger().info(
            f"  lat/lon bounds: ({min_lat:f}, {min_lon:f}) - ({max_lat:f}, {max_lon:f})")
        return GeoReference(geofile, min_lat, min_lon, max_lat, max_lon, area)

    def select_tileset(self, lat, lon):
        # Select the geofile that contains the current point.  If multiple
        # geo-files contain the point, select the largest.  If no geo-files
        # contain the point, select the closest one.
        max_area = 0
        path = None
        for georef in self.georeferences:
            if lat > georef.min_lat and lat < georef.max_lat and \
                    lon > georef.min_lon and lon < georef.max_lon and georef.area > max_area:
                path = georef.path
                max_area = georef.area

        if path is None:
            min_dist = float("inf")
            for georef in self.georeferences:
                clat = (georef.min_lat + georef.max_lat) / 2.0
                clon = (georef.min_lon + georef.max_lon) / 2.0
                dist = distance(clat, clon, lat, lon)
                if dist < min_dist:
                    min_dist = dist
                    path = georef.path
        return path

    def update_tileset(self, path):
        # If the geo-file has changed, call the service for adding/updating
        # the mapviz display.
        if path is None or path == self._last_path or self._request_pending:
            return
        self.get_logger().info(f"updating tileset: {path}")
        request = AddMapvizDisplay.Request()
        request.type = 'mapviz_plugins/multires_image'
        request.draw_order = self.draw_order
        request.name = self.display_name
        request.visible = True
        request.properties.append(KeyValue(key='path', value=path))
        self._request_pending = True
        future = self._client.call_async(request)
        future.add_done_callback(lambda f: self.on_tileset_response(f, path))

    def on_tileset_response(self, future, path):
        self._request_pending = False
        response = future.result()
        if response.success:
            self._last_path = path
        else:
            self.get_logger().warn(f"failed to update tileset: {response.message}")

    def on_timer(self):
        if self.use_local_xy:
            if self._last_path is None and self.georeferences and self._local_xy is not None:
                path = self.select_tileset(self._local_xy.pose.position.y,
                                           self._local_xy.pose.position.x)
                self.update_tileset(path)
        else:
            if self._gps_fix is not None:
                path = self.select_tileset(self._gps_fix.latitude, self._gps_fix.longitude)
                self.update_tileset(path)
            else:
                self.get_logger().info(
                    f"waiting for gps message: {self.resolve_topic_name('gps')}")


def main(args=None):
    rclpy.init(args=args)
    node = MapvizTileLoader()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()
