#!/usr/bin/env python3

# Copyright 2021 Tier IV, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import hashlib
import pathlib

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy
from rclpy.qos import QoSProfile
from tier4_external_api_msgs.msg import MapHash
from tier4_external_api_msgs.msg import ResponseStatus
from tier4_external_api_msgs.srv import GetTextFile


class MapHashGenerator(Node):
    def __init__(self):
        super().__init__("map_hash_generator")
        self.lanelet_path = self.declare_parameter("lanelet2_map_path", "").value
        self.lanelet_text = self.load_lanelet_text(self.lanelet_path)
        self.lanelet_hash = self.generate_lanelet_file_hash(self.lanelet_text)

        self.pcd_map_path = self.declare_parameter("pointcloud_map_path", "").value
        self.pcd_map_hash = self.generate_pcd_file_hash(self.pcd_map_path)

        qos_profile = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
        msg = MapHash()
        msg.lanelet = self.lanelet_hash
        msg.pcd = self.pcd_map_hash
        self.pub = self.create_publisher(MapHash, "/api/autoware/get/map/info/hash", qos_profile)
        self.pub.publish(msg)

        self.srv = self.create_service(
            GetTextFile, "/api/autoware/get/map/lanelet/xml", self.on_get_lanelet_xml
        )  # noqa: E501

    def on_get_lanelet_xml(self, request, response):
        response.status.code = ResponseStatus.SUCCESS
        response.file.text = self.lanelet_text
        return response

    @staticmethod
    def load_lanelet_text(path):
        path = pathlib.Path(path)
        return path.read_text() if path.is_file() else ""

    @staticmethod
    def generate_lanelet_file_hash(data):
        return hashlib.sha256(data.encode()).hexdigest() if data else ""

    def update_hash(self, m, path):
        try:
            with open(path, "rb") as pcd_file:
                m.update(pcd_file.read())
        except FileNotFoundError as e:
            self.get_logger().error(e)
            return False
        return True

    def generate_pcd_file_hash(self, path):
        path = pathlib.Path(path)
        if path.is_file():
            if not path.suffix == ".pcd":
                self.get_logger().error(f"[{path}] is not pcd file")
                return ""
            m = hashlib.sha256()
            if not self.update_hash(m, path):
                return ""
            return m.hexdigest()

        if path.is_dir():
            m = hashlib.sha256()
            for pcd_file_path in sorted(path.iterdir()):
                if not pcd_file_path.suffix == ".pcd":
                    continue
                if not self.update_hash(m, pcd_file_path):
                    return ""
            if m.hexdigest() == hashlib.sha256().hexdigest():
                self.get_logger().error(f"there are no pcd files in [{path}]")
                return ""
            return m.hexdigest()

        self.get_logger().error(f"[{path}] is neither file nor directory")
        return ""


def main(args=None):
    rclpy.init(args=args)
    node = MapHashGenerator()
    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        pass
