// Copyright 2023 Leidos
//
// 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.

#include "carma_cooperative_perception/host_vehicle_filter_component.hpp"

#include <carma_cooperative_perception_interfaces/msg/detection.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <rclcpp_components/register_node_macro.hpp>

#include <vector>

namespace carma_cooperative_perception
{
auto HostVehicleFilterNode::handle_on_configure(
  const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn
{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: configuring");

  detection_list_sub_ = create_subscription<
    carma_cooperative_perception_interfaces::msg::DetectionList>(
    "input/detection_list", 1,
    [this](const carma_cooperative_perception_interfaces::msg::DetectionList::SharedPtr msg_ptr) {
      if (const auto current_state{this->get_current_state().label()}; current_state == "active") {
        attempt_filter_and_republish(*msg_ptr);
      } else {
        RCLCPP_WARN(
          this->get_logger(),
          "Trying to receive message on the topic '%s', but the containing node is not activated. "
          "Current node state: '%s'",
          this->detection_list_sub_->get_topic_name(), current_state.c_str());
      }
    });

  host_vehicle_pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
    "input/host_vehicle_pose", 1, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg_ptr) {
      if (const auto current_state{this->get_current_state().label()}; current_state == "active") {
        update_host_vehicle_pose(*msg_ptr);
      } else {
        RCLCPP_WARN(
          this->get_logger(),
          "Trying to receive message on the topic '%s', but the containing node is not activated. "
          "Current node state: '%s'",
          this->detection_list_sub_->get_topic_name(), current_state.c_str());
      }
    });

  detection_list_pub_ =
    create_publisher<carma_cooperative_perception_interfaces::msg::DetectionList>(
      "output/detection_list", 1);

  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured");

  on_set_parameters_callback_ =
    add_on_set_parameters_callback([this](const std::vector<rclcpp::Parameter> & parameters) {
      rcl_interfaces::msg::SetParametersResult result;
      result.successful = true;
      result.reason = "success";

      for (const auto & parameter : parameters) {
        if (parameter.get_name() == "distance_threshold_meters") {
          if (this->get_current_state().label() == "active") {
            result.successful = false;
            result.reason = "parameter is read-only while node is in 'Active' state";

            RCLCPP_ERROR(
              get_logger(),
              ("Cannot change parameter 'distance_threshold_meters': " + result.reason).c_str());

            break;
          }

          if (const auto value{parameter.as_double()}; value < 0) {
            result.successful = false;
            result.reason = "parameter must be nonnegative";

            RCLCPP_ERROR(
              get_logger(),
              ("Cannot change parameter 'distance_threshold_meters': " + result.reason).c_str());

            break;
          } else {
            this->squared_distance_threshold_meters_ = std::pow(value, 2);
          }
        } else {
          result.successful = false;
          result.reason = "Unexpected parameter name '" + parameter.get_name() + '\'';
          break;
        }
      }

      return result;
    });

  declare_parameter("distance_threshold_meters", 0.0);

  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

auto HostVehicleFilterNode::handle_on_activate(const rclcpp_lifecycle::State & /* previous_state */)
  -> carma_ros2_utils::CallbackReturn
{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: activating");
  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully activated");

  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

auto HostVehicleFilterNode::handle_on_deactivate(
  const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn
{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: deactivating");
  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully deactivated");

  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

auto HostVehicleFilterNode::handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */)
  -> carma_ros2_utils::CallbackReturn
{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: cleaning up");

  // CarmaLifecycleNode does not handle subscriber pointer reseting for us
  detection_list_sub_.reset();
  host_vehicle_pose_sub_.reset();

  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully cleaned up");

  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

auto HostVehicleFilterNode::handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */)
  -> carma_ros2_utils::CallbackReturn
{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: shutting down");

  // CarmaLifecycleNode does not handle subscriber pointer reseting for us
  detection_list_sub_.reset();
  host_vehicle_pose_sub_.reset();

  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully shut down");

  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

auto HostVehicleFilterNode::update_host_vehicle_pose(const geometry_msgs::msg::PoseStamped & msg)
  -> void
{
  host_vehicle_pose_ = msg;
}

auto HostVehicleFilterNode::attempt_filter_and_republish(
  carma_cooperative_perception_interfaces::msg::DetectionList msg) -> void
{
  if (!host_vehicle_pose_.has_value()) {
    RCLCPP_WARN(get_logger(), "Could not filter detection list: host vehicle pose unknown");
    return;
  }

  const auto is_within_distance = [this](const auto & detection) {
    if (
      detection.semantic_class != detection.SEMANTIC_CLASS_UNKNOWN &&
      detection.semantic_class != detection.SEMANTIC_CLASS_SMALL_VEHICLE) {
      return false;
    }

    return euclidean_distance_squared(host_vehicle_pose_.value().pose, detection.pose.pose) <=
           this->squared_distance_threshold_meters_;
  };

  const auto new_end{
    std::remove_if(std::begin(msg.detections), std::end(msg.detections), is_within_distance)};

  msg.detections.erase(new_end, std::end(msg.detections));

  this->detection_list_pub_->publish(msg);
}

auto euclidean_distance_squared(
  const geometry_msgs::msg::Pose & a, const geometry_msgs::msg::Pose & b) -> double
{
  return std::pow(a.position.x - b.position.x, 2) + std::pow(a.position.y - b.position.y, 2) +
         std::pow(a.position.z - b.position.z, 2);
}

}  // namespace carma_cooperative_perception

// This is not our macro, so we should not worry about linting it.

RCLCPP_COMPONENTS_REGISTER_NODE(                        // NOLINT
  carma_cooperative_perception::HostVehicleFilterNode)  // NOLINT
